Skip to content

Commit

Permalink
added set_fov function to omnicam and improved drawray
Browse files Browse the repository at this point in the history
  • Loading branch information
@teksander
teksander committed Jun 30, 2023
1 parent ad9fc3b commit e472446
Show file tree
Hide file tree
Showing 6 changed files with 71 additions and 25 deletions.
6 changes: 3 additions & 3 deletions examples/aggregation_epuck.argos
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
<controllers>


<python_controller id="python" library="/mnt/c/Users/albyr/Documents/argos-python/libpy_controller_interface.so">
<python_controller id="python" library="/home/eksander/geth-argos/argos-python/build/libpy_controller_interface.so">
<!-- Normal actuator/sensor configuration follows -->
<actuators>
<epuck_wheels implementation="default" />
Expand All @@ -28,7 +28,7 @@
<!-- No required configuration -->

<!-- Optionally, you can pass a script as a controller parameter: -->
<params script="/mnt/c/Users/albyr/Documents/argos-python/examples/aggregation_epuck.py" />
<params script="/home/eksander/geth-argos/argos-python/examples/aggregation_epuck.py" />
</python_controller>

</controllers>
Expand All @@ -37,7 +37,7 @@
<!-- * Arena configuration * -->
<!-- *********************** -->
<arena size="5, 5, 1" center="0,0,0.5">
<floor id="f" source="image" path="/mnt/c/Users/albyr/Documents/argos-python/examples/one_spot.png" />
<floor id="f" source="image" path="/home/eksander/geth-argos/argos-python/examples/one_spot.png" />

<box id="bn" size="0.1, 5, 0.2" movable="false">
<body position="2.5,0,0" orientation="0,0,0" />
Expand Down
34 changes: 19 additions & 15 deletions py_actusensor_wrapper_generic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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) {
Expand Down
9 changes: 7 additions & 2 deletions py_actusensor_wrapper_generic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};

/****************************************/
Expand Down
44 changes: 40 additions & 4 deletions py_environment_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Real>(boost::python::object(c_start[0])),
// boost::python::extract<Real>(boost::python::object(c_start[1])),
// boost::python::extract<Real>(boost::python::object(c_start[2])));

// c_end_vec = CVector3(boost::python::extract<Real>(boost::python::object(c_end[0])),
// boost::python::extract<Real>(boost::python::object(c_end[1])),
// boost::python::extract<Real>(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;
Expand All @@ -76,9 +96,25 @@ void CQTOpenGLUserFunctionsWrapper::DrawRay(const boost::python::list c_start,
boost::python::extract<Real>(boost::python::object(c_end[1])),
boost::python::extract<Real>(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<std::string>(str_color_name).check()) {
// Handle string color name
std::string color_name = boost::python::extract<std::string>(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<boost::python::list>(str_color_name).check()) {
// Handle RGB array
boost::python::list rgb_list = boost::python::extract<boost::python::list>(str_color_name);
Real r = boost::python::extract<Real>(rgb_list[0]);
Real g = boost::python::extract<Real>(rgb_list[1]);
Real b = boost::python::extract<Real>(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,
Expand Down
2 changes: 1 addition & 1 deletion py_environment_wrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
1 change: 1 addition & 0 deletions py_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_<CPerspectiveCameraWrapper, boost::noncopyable>("perspective_camera_wrapper",
Expand Down

0 comments on commit e472446

Please sign in to comment.