Skip to content

Commit

Permalink
added option to draw using RGB color
Browse files Browse the repository at this point in the history
  • Loading branch information
@teksander
teksander committed Jul 11, 2023
1 parent e472446 commit 993f2d2
Show file tree
Hide file tree
Showing 2 changed files with 85 additions and 21 deletions.
102 changes: 83 additions & 19 deletions py_environment_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,16 +22,47 @@ const std::string CQTOpenGLUserFunctionsWrapper::GetDrawList(const std::string&
void CQTOpenGLUserFunctionsWrapper::DrawCircle(const boost::python::list c_position_list,
const boost::python::list c_orientation_list,
const Real f_radius,
const std::string str_color_name,
const boost::python::object color_str_list,
const bool b_fill) {
m_pcCQTOpenGLUserFunctions->DrawCircle(
CVector3(boost::python::extract<Real>(boost::python::object(c_position_list[0])),
boost::python::extract<Real>(boost::python::object(c_position_list[1])),
boost::python::extract<Real>(boost::python::object(c_position_list[2]))),
CQuaternion(), // Implement orientation
f_radius,
ActusensorsWrapper::CColorWrapper(str_color_name).m_cColor,
b_fill);

CVector3 c_position;

c_position = CVector3(boost::python::extract<Real>(boost::python::object(c_position_list[0])),
boost::python::extract<Real>(boost::python::object(c_position_list[1])),
boost::python::extract<Real>(boost::python::object(c_position_list[2])));

if (boost::python::extract<std::string>(color_str_list).check()) {
// Handle string color name
std::string color_str = boost::python::extract<std::string>(color_str_list);

m_pcCQTOpenGLUserFunctions->DrawCircle(
c_position,
CQuaternion(), // Implement orientation
f_radius,
ActusensorsWrapper::CColorWrapper(color_str).m_cColor,
b_fill);

} else if (boost::python::extract<boost::python::list>(color_str_list).check()) {
// Handle RGB array
boost::python::list rgb_list = boost::python::extract<boost::python::list>(color_str_list);
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->DrawCircle(
c_position,
CQuaternion(), // Implement orientation
f_radius,
ActusensorsWrapper::CColorWrapper(r,g,b).m_cColor,
b_fill);

} else {
// Invalid argument type for color
// Handle the error accordingly
}



}

void CQTOpenGLUserFunctionsWrapper::DrawPolygon(const boost::python::list c_position_list,
Expand Down Expand Up @@ -120,16 +151,49 @@ void CQTOpenGLUserFunctionsWrapper::DrawRay(const boost::python::list c_start,
void CQTOpenGLUserFunctionsWrapper::DrawBox(const boost::python::list c_position_list,
const boost::python::list c_orientation_list,
const boost::python::list c_size_list,
const std::string str_color_name) {
m_pcCQTOpenGLUserFunctions->DrawBox(
CVector3(boost::python::extract<Real>(boost::python::object(c_position_list[0])),
boost::python::extract<Real>(boost::python::object(c_position_list[1])),
boost::python::extract<Real>(boost::python::object(c_position_list[2]))),
CQuaternion(), // Implement orientation
CVector3(boost::python::extract<Real>(boost::python::object(c_size_list[0])),
boost::python::extract<Real>(boost::python::object(c_size_list[1])),
boost::python::extract<Real>(boost::python::object(c_size_list[2]))),
ActusensorsWrapper::CColorWrapper(str_color_name).m_cColor);
const boost::python::object color_str_list) {

CVector3 c_pos_vec;
CVector3 c_siz_vec;

c_pos_vec = CVector3(boost::python::extract<Real>(boost::python::object(c_position_list[0])),
boost::python::extract<Real>(boost::python::object(c_position_list[1])),
boost::python::extract<Real>(boost::python::object(c_position_list[2])));

c_siz_vec = CVector3(boost::python::extract<Real>(boost::python::object(c_size_list[0])),
boost::python::extract<Real>(boost::python::object(c_size_list[1])),
boost::python::extract<Real>(boost::python::object(c_size_list[2])));

if (boost::python::extract<std::string>(color_str_list).check()) {
// Handle string color name
std::string color_str = boost::python::extract<std::string>(color_str_list);

m_pcCQTOpenGLUserFunctions->DrawBox(c_pos_vec,
CQuaternion(), // To-do: Implement orientation
c_siz_vec,
ActusensorsWrapper::CColorWrapper(color_str).m_cColor);

} else if (boost::python::extract<boost::python::list>(color_str_list).check()) {
// Handle RGB array
boost::python::list rgb_list = boost::python::extract<boost::python::list>(color_str_list);
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->DrawBox(c_pos_vec,
CQuaternion(), // To-do: Implement orientation
c_siz_vec,
ActusensorsWrapper::CColorWrapper(r,g,b).m_cColor);
} else {
// Invalid argument type for color
// Handle the error accordingly
}

// m_pcCQTOpenGLUserFunctions->DrawBox(
// c_pos_vec,
// CQuaternion(), // To-do: Implement orientation
// c_siz_vec,
// ActusensorsWrapper::CColorWrapper(color_str_list).m_cColor);
}

void CQTOpenGLUserFunctionsWrapper::DrawCylinder(const boost::python::list c_position_list,
Expand Down
4 changes: 2 additions & 2 deletions py_environment_wrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ namespace argos {
const boost::python::list c_position_list,
const boost::python::list c_orientation_list,
const Real f_radius,
const std::string str_color_name,
const boost::python::object color_str_list,
const bool b_fill=true);

void DrawPolygon(
Expand All @@ -79,7 +79,7 @@ namespace argos {
const boost::python::list c_position_list,
const boost::python::list c_orientation_list,
const boost::python::list c_size_list,
const std::string str_color_name);
const boost::python::object str_color_name);

void DrawText(
const boost::python::list c_position,
Expand Down

0 comments on commit 993f2d2

Please sign in to comment.