From e4724465ac585bf979ad043fd8ef6c7400170b79 Mon Sep 17 00:00:00 2001 From: teksander Date: Fri, 30 Jun 2023 12:44:29 +0200 Subject: [PATCH] added set_fov function to omnicam and improved drawray --- examples/aggregation_epuck.argos | 6 ++--- py_actusensor_wrapper_generic.cpp | 34 +++++++++++++----------- py_actusensor_wrapper_generic.h | 9 +++++-- py_environment_wrapper.cpp | 44 ++++++++++++++++++++++++++++--- py_environment_wrapper.h | 2 +- py_wrapper.cpp | 1 + 6 files changed, 71 insertions(+), 25 deletions(-) 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",