diff --git a/py_environment_wrapper.cpp b/py_environment_wrapper.cpp index 1bdc7a1..f8bcf2c 100644 --- a/py_environment_wrapper.cpp +++ b/py_environment_wrapper.cpp @@ -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(boost::python::object(c_position_list[0])), - boost::python::extract(boost::python::object(c_position_list[1])), - boost::python::extract(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(boost::python::object(c_position_list[0])), + boost::python::extract(boost::python::object(c_position_list[1])), + boost::python::extract(boost::python::object(c_position_list[2]))); + + if (boost::python::extract(color_str_list).check()) { + // Handle string color name + std::string color_str = boost::python::extract(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(color_str_list).check()) { + // Handle RGB array + boost::python::list rgb_list = boost::python::extract(color_str_list); + 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->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, @@ -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(boost::python::object(c_position_list[0])), - boost::python::extract(boost::python::object(c_position_list[1])), - boost::python::extract(boost::python::object(c_position_list[2]))), - CQuaternion(), // Implement orientation - CVector3(boost::python::extract(boost::python::object(c_size_list[0])), - boost::python::extract(boost::python::object(c_size_list[1])), - boost::python::extract(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(boost::python::object(c_position_list[0])), + boost::python::extract(boost::python::object(c_position_list[1])), + boost::python::extract(boost::python::object(c_position_list[2]))); + + c_siz_vec = CVector3(boost::python::extract(boost::python::object(c_size_list[0])), + boost::python::extract(boost::python::object(c_size_list[1])), + boost::python::extract(boost::python::object(c_size_list[2]))); + + if (boost::python::extract(color_str_list).check()) { + // Handle string color name + std::string color_str = boost::python::extract(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(color_str_list).check()) { + // Handle RGB array + boost::python::list rgb_list = boost::python::extract(color_str_list); + 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->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, diff --git a/py_environment_wrapper.h b/py_environment_wrapper.h index 2446658..ff2ff14 100644 --- a/py_environment_wrapper.h +++ b/py_environment_wrapper.h @@ -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( @@ -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,