ok layers

roboticslab-uc3m · Sep 18, 2023 · d764266 · d764266
1 parent 79ae76a
commit d764266
Showing 1 changed file with 38 additions and 19 deletions.
diff --git 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<OpenRAVE::AABB> 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<float>(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<OpenRAVE::AABB> 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<float>(0.0, 0.0, 1.0));
-                        else
-                            objKinBodyPtr->GetLinks()[0]->GetGeometry(0)->SetDiffuseColor(OpenRAVE::RaveVector<float>(1.0, 1.0, 1.0));
-    #if OPENRAVE_VERSION >= OPENRAVE_VERSION_COMBINED(0, 67, 0)
+                        objKinBodyPtr->GetLinks()[0]->GetGeometry(0)->SetDiffuseColor(OpenRAVE::RaveVector<float>(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);
                     }
                 }