From d764266bd7b5c1d27eea9ac80ae9d9b51888173a Mon Sep 17 00:00:00 2001 From: jgvictores Date: Mon, 18 Sep 2023 18:37:35 +0200 Subject: [PATCH] ok layers --- .../OpenraveYarpIroning.cpp | 57 ++++++++++++------- 1 file changed, 38 insertions(+), 19 deletions(-) diff --git a/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp b/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp index 526afddd..2a7a7f8b 100644 --- a/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp +++ b/libraries/OpenravePlugins/OpenraveYarpIroning/OpenraveYarpIroning.cpp @@ -164,24 +164,46 @@ class OpenraveYarpIroning : public OpenRAVE::ModuleBase, double tableX = 0.4; double tableY = 0.4; + //-- layer 1 for (unsigned int sboxIdxX = 0; sboxIdxX < squaresX; sboxIdxX++) { for (unsigned int sboxIdxY = 0; sboxIdxY < squaresY; sboxIdxY++) { - if((intMap[sboxIdxX][sboxIdxY]==1)||(intMap[sboxIdxX][sboxIdxY]==2)) - { + if ((intMap[sboxIdxX][sboxIdxY] == 1) || (intMap[sboxIdxX][sboxIdxY] == 2)) + { OpenRAVE::KinBodyPtr objKinBodyPtr = OpenRAVE::RaveCreateKinBody(_penv, ""); std::vector boxes(1); - if(intMap[sboxIdxX][sboxIdxY]==2) - { - boxes[0].extents = OpenRAVE::Vector(tableX / (2.0 * squaresX), tableY / (2.0 * squaresY), 0.01); - boxes[0].pos = OpenRAVE::Vector(0.6 + (0.5 + sboxIdxX) * tableX / squaresX, -(tableY / 2.0) + (0.5 + sboxIdxY) * tableY / squaresY, 0.01-0.1); - } - else - { - boxes[0].extents = OpenRAVE::Vector(tableX / (2.0 * squaresX), tableY / (2.0 * squaresY), 0.005); - boxes[0].pos = OpenRAVE::Vector(0.6 + (0.5 + sboxIdxX) * tableX / squaresX, -(tableY / 2.0) + (0.5 + sboxIdxY) * tableY / squaresY, 0.005-0.1); - } + boxes[0].extents = OpenRAVE::Vector(tableX / (2.0 * squaresX), tableY / (2.0 * squaresY), 0.005); + boxes[0].pos = OpenRAVE::Vector(0.6 + (0.5 + sboxIdxX) * tableX / squaresX, -(tableY / 2.0) + (0.5 + sboxIdxY) * tableY / squaresY, 0.005 - 0.1); + objKinBodyPtr->InitFromBoxes(boxes, true); + std::string objName("sbox_w_"); + std::ostringstream s; + s << sboxIdxX; + s << "_"; + s << sboxIdxY; + objName.append(s.str()); + objKinBodyPtr->SetName(objName); + objKinBodyPtr->GetLinks()[0]->GetGeometry(0)->SetDiffuseColor(OpenRAVE::RaveVector(1.0, 1.0, 1.0)); +#if OPENRAVE_VERSION >= OPENRAVE_VERSION_COMBINED(0, 67, 0) + _penv->Add(objKinBodyPtr, OpenRAVE::IAM_AllowRenaming); +#else + _penv->Add(objKinBodyPtr, true); +#endif + _objKinBodyPtrs.push_back(objKinBodyPtr); + } + } + } + //-- layer 2 + for (unsigned int sboxIdxX = 0; sboxIdxX < squaresX; sboxIdxX++) + { + for (unsigned int sboxIdxY = 0; sboxIdxY < squaresY; sboxIdxY++) + { + if (intMap[sboxIdxX][sboxIdxY] == 2) + { + OpenRAVE::KinBodyPtr objKinBodyPtr = OpenRAVE::RaveCreateKinBody(_penv, ""); + std::vector boxes(1); + boxes[0].extents = OpenRAVE::Vector(tableX / (2.0 * squaresX), tableY / (2.0 * squaresY), 0.005); + boxes[0].pos = OpenRAVE::Vector(0.6 + (0.5 + sboxIdxX) * tableX / squaresX, -(tableY / 2.0) + (0.5 + sboxIdxY) * tableY / squaresY, 0.015 - 0.1); objKinBodyPtr->InitFromBoxes(boxes, true); std::string objName("sbox_"); std::ostringstream s; @@ -190,15 +212,12 @@ class OpenraveYarpIroning : public OpenRAVE::ModuleBase, s << sboxIdxY; objName.append(s.str()); objKinBodyPtr->SetName(objName); - if(intMap[sboxIdxX][sboxIdxY]==2) - objKinBodyPtr->GetLinks()[0]->GetGeometry(0)->SetDiffuseColor(OpenRAVE::RaveVector(0.0, 0.0, 1.0)); - else - objKinBodyPtr->GetLinks()[0]->GetGeometry(0)->SetDiffuseColor(OpenRAVE::RaveVector(1.0, 1.0, 1.0)); - #if OPENRAVE_VERSION >= OPENRAVE_VERSION_COMBINED(0, 67, 0) + objKinBodyPtr->GetLinks()[0]->GetGeometry(0)->SetDiffuseColor(OpenRAVE::RaveVector(0.0, 0.0, 1.0)); +#if OPENRAVE_VERSION >= OPENRAVE_VERSION_COMBINED(0, 67, 0) _penv->Add(objKinBodyPtr, OpenRAVE::IAM_AllowRenaming); - #else +#else _penv->Add(objKinBodyPtr, true); - #endif +#endif _objKinBodyPtrs.push_back(objKinBodyPtr); } }