diff --git a/examples/aggregation_epuck.argos b/examples/aggregation_epuck.argos
index 5e9187c..1f09967 100644
--- a/examples/aggregation_epuck.argos
+++ b/examples/aggregation_epuck.argos
@@ -14,7 +14,7 @@
-
+
@@ -28,7 +28,7 @@
-
+
@@ -37,7 +37,7 @@
-
+
diff --git a/py_actusensor_wrapper_generic.cpp b/py_actusensor_wrapper_generic.cpp
index bda4b10..feec966 100644
--- a/py_actusensor_wrapper_generic.cpp
+++ b/py_actusensor_wrapper_generic.cpp
@@ -90,8 +90,12 @@ Real CPositioningSensorWrapper::GetOrientation() const {
/****************************************/
/****************************************/
+COmnidirectionalCameraWrapper::COmnidirectionalCameraWrapper()
+ : m_maxAngle(3.14) {}
-COmnidirectionalCameraWrapper::COmnidirectionalCameraWrapper() {}
+void COmnidirectionalCameraWrapper::setFOV(double max_angle) {
+ m_maxAngle = max_angle;
+}
boost::python::list COmnidirectionalCameraWrapper::GetReadings() const {
if (m_pcOmniCam == nullptr) {
@@ -101,28 +105,28 @@ boost::python::list COmnidirectionalCameraWrapper::GetReadings() const {
}
boost::python::list readings;
-
+
for (size_t i = 0; i < m_pcOmniCam->GetReadings().BlobList.size(); ++i) {
- boost::python::list color;
- boost::python::list reading;
+ double angle = m_pcOmniCam->GetReadings().BlobList[i]->Angle.GetValue();
+ if (angle < m_maxAngle && angle > -m_maxAngle) {
+ boost::python::list color;
+ boost::python::list reading;
- color.append(m_pcOmniCam->GetReadings().BlobList[i]->Color.GetRed());
- color.append(m_pcOmniCam->GetReadings().BlobList[i]->Color.GetGreen());
- color.append(m_pcOmniCam->GetReadings().BlobList[i]->Color.GetBlue());
+ color.append(m_pcOmniCam->GetReadings().BlobList[i]->Color.GetRed());
+ color.append(m_pcOmniCam->GetReadings().BlobList[i]->Color.GetGreen());
+ color.append(m_pcOmniCam->GetReadings().BlobList[i]->Color.GetBlue());
- reading.append(color);
- reading.append(m_pcOmniCam->GetReadings().BlobList[i]->Angle.GetValue());
- reading.append(m_pcOmniCam->GetReadings().BlobList[i]->Distance);
+ reading.append(color);
+ reading.append(angle);
+ reading.append(m_pcOmniCam->GetReadings().BlobList[i]->Distance);
- readings.append(reading);
+ readings.append(reading);
+ }
}
-
return readings;
-
- // In the way below I was unable to read color on python. angle.value() and distance are ok
- // return ActusensorsWrapper::ToPythonList(m_pcOmniCam->GetReadings().BlobList);
}
+
// Enable the camera.
void COmnidirectionalCameraWrapper::Enable() {
if (m_pcOmniCam == nullptr) {
diff --git a/py_actusensor_wrapper_generic.h b/py_actusensor_wrapper_generic.h
index 18de7c1..d8f2989 100644
--- a/py_actusensor_wrapper_generic.h
+++ b/py_actusensor_wrapper_generic.h
@@ -97,9 +97,14 @@ class COmnidirectionalCameraWrapper {
// Disable the camera.
void Disable();
- // Return the number of readings obtained so far, i.e. the number of control steps from which
- // the recording started.
+ // Set the field of vision.
+ void setFOV(double max_angle);
+
+ // Return the number of readings obtained so far
const int GetCounter() const;
+
+ private:
+ double m_maxAngle;
};
/****************************************/
diff --git a/py_environment_wrapper.cpp b/py_environment_wrapper.cpp
index 3c21308..1bdc7a1 100644
--- a/py_environment_wrapper.cpp
+++ b/py_environment_wrapper.cpp
@@ -61,9 +61,29 @@ void CQTOpenGLUserFunctionsWrapper::DrawPolygon(const boost::python::list c_posi
b_fill);
}
+// void CQTOpenGLUserFunctionsWrapper::DrawRay(const boost::python::list c_start,
+// const boost::python::list c_end,
+// const std::string str_color_name,
+// const Real f_width) {
+// CVector3 c_stt_vec;
+// CVector3 c_end_vec;
+
+// c_stt_vec = CVector3(boost::python::extract(boost::python::object(c_start[0])),
+// boost::python::extract(boost::python::object(c_start[1])),
+// boost::python::extract(boost::python::object(c_start[2])));
+
+// c_end_vec = CVector3(boost::python::extract(boost::python::object(c_end[0])),
+// boost::python::extract(boost::python::object(c_end[1])),
+// boost::python::extract(boost::python::object(c_end[2])));
+
+// m_pcCQTOpenGLUserFunctions->DrawRay(CRay3(c_stt_vec, c_end_vec),
+// ActusensorsWrapper::CColorWrapper(str_color_name).m_cColor,
+// f_width);
+// }
+
void CQTOpenGLUserFunctionsWrapper::DrawRay(const boost::python::list c_start,
const boost::python::list c_end,
- const std::string str_color_name,
+ const boost::python::object str_color_name,
const Real f_width) {
CVector3 c_stt_vec;
CVector3 c_end_vec;
@@ -76,9 +96,25 @@ void CQTOpenGLUserFunctionsWrapper::DrawRay(const boost::python::list c_start,
boost::python::extract(boost::python::object(c_end[1])),
boost::python::extract(boost::python::object(c_end[2])));
- m_pcCQTOpenGLUserFunctions->DrawRay(CRay3(c_stt_vec, c_end_vec),
- ActusensorsWrapper::CColorWrapper(str_color_name).m_cColor,
- f_width);
+ if (boost::python::extract(str_color_name).check()) {
+ // Handle string color name
+ std::string color_name = boost::python::extract(str_color_name);
+ m_pcCQTOpenGLUserFunctions->DrawRay(CRay3(c_stt_vec, c_end_vec),
+ ActusensorsWrapper::CColorWrapper(color_name).m_cColor,
+ f_width);
+ } else if (boost::python::extract(str_color_name).check()) {
+ // Handle RGB array
+ boost::python::list rgb_list = boost::python::extract(str_color_name);
+ Real r = boost::python::extract(rgb_list[0]);
+ Real g = boost::python::extract(rgb_list[1]);
+ Real b = boost::python::extract(rgb_list[2]);
+ m_pcCQTOpenGLUserFunctions->DrawRay(CRay3(c_stt_vec, c_end_vec),
+ ActusensorsWrapper::CColorWrapper(r,g,b).m_cColor,
+ f_width);
+ } else {
+ // Invalid argument type for color
+ // Handle the error accordingly
+ }
}
void CQTOpenGLUserFunctionsWrapper::DrawBox(const boost::python::list c_position_list,
diff --git a/py_environment_wrapper.h b/py_environment_wrapper.h
index 12650ea..2446658 100644
--- a/py_environment_wrapper.h
+++ b/py_environment_wrapper.h
@@ -65,7 +65,7 @@ namespace argos {
void DrawRay(
const boost::python::list c_start,
const boost::python::list c_end,
- const std::string str_color_name,
+ const boost::python::object str_color_name,
const Real f_width);
void DrawCylinder(
diff --git a/py_wrapper.cpp b/py_wrapper.cpp
index 92b6025..b4cc793 100644
--- a/py_wrapper.cpp
+++ b/py_wrapper.cpp
@@ -325,6 +325,7 @@ BOOST_PYTHON_MODULE(libpy_controller_interface) {
.def("enable", &COmnidirectionalCameraWrapper::Enable)
.def("disable", &COmnidirectionalCameraWrapper::Disable)
.def("get_readings", &COmnidirectionalCameraWrapper::GetReadings)
+ .def("set_fov", &COmnidirectionalCameraWrapper::setFOV)
.def("get_counter", &COmnidirectionalCameraWrapper::GetCounter);
class_("perspective_camera_wrapper",