bugfix: using "info (returned value)" inside lambda. Didn't know that this was even possible

source/armarx/manipulation/components/articulated_object_augmented_visualization/Component.cpp

@@ -56,7 +56,6 @@
 #include "armarx/manipulation/core/types.h"
 #include "armarx/manipulation/geometric_planning/ArVizDrawer.h"
 
-
 namespace armarx::manipulation::components::articulated_object_augmented_visualization
 {
     Component::Component()
@@ -65,10 +64,8 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
         addPlugin(virtualRobotReaderPlugin);
     }
 
-
     const std::string Component::defaultName = "articulated_object_augmented_visualization";
 
-
     armarx::PropertyDefinitionsPtr
     Component::createPropertyDefinitions()
     {
@@ -81,13 +78,11 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
         return def;
     }
 
-
     void
     Component::onInitComponent()
     {
     }
 
-
     void
     Component::onConnectComponent()
     {
@@ -100,7 +95,7 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
             for (const auto& description : descriptions)
             {
                 ARMARX_IMPORTANT << "Loading robot `" << description.name << "` from "
-                               << description.xml;
+                                 << description.xml;
                 const auto robot = VirtualRobot::RobotIO::loadRobot(
                     description.xml.toSystemPath(),
                     VirtualRobot::RobotIO::RobotDescription::eStructure);
@@ -129,13 +124,11 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
         }
     }
 
-
     void
     Component::onDisconnectComponent()
     {
     }
 
-
     void
     Component::onExitComponent()
     {
@@ -208,7 +201,7 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
 
 
         ARMARX_VERBOSE << "Found " << articulatedObjectPoses.size()
-                    << " articulated objects in the scene";
+                       << " articulated objects in the scene";
 
         for (const auto& objectPose : articulatedObjectPoses)
         {
@@ -238,21 +231,18 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
         return articulatedObjects;
     }
 
-
     std::string
     Component::getDefaultName() const
     {
         return Component::defaultName;
     }
 
-
     std::string
     Component::GetDefaultName()
     {
         return Component::defaultName;
     }
 
-
     void
     Component::visualizeAllParametricPathsForNodes(const std::string& identifier)
     {
@@ -298,24 +288,37 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
 
             if (properties.drawGrasps)
             {
+                ARMARX_INFO << "Visualizing grasp for object "
+                            << articulatedObject->getType() + "/" + articulatedObject->getName();
+
                 auto layer = arviz.layer("grasps " + articulatedObject->getType() + "/" +
                                          articulatedObject->getName());
 
-                for (const auto& [node, graspSets] : simox::geometric_planning::allGraspSets(*articulatedObject))
+                for (const auto& [node, graspSets] :
+                     simox::geometric_planning::allGraspSets(*articulatedObject))
                 {
+                    ARMARX_CHECK_NOT_NULL(node);
+
                     if (not graspSets.empty())
                     {
                         ARMARX_VERBOSE << "Found grasp sets attached to node " << node->getName();
                         for (const auto& graspSet : graspSets)
                         {
+                            ARMARX_CHECK_NOT_NULL(graspSet);
+
                             for (const auto& grasp : graspSet->getGrasps())
                             {
                                 const std::string side = graspSet->getName();
 
                                 ARMARX_TRACE;
-                                ARMARX_VERBOSE << node->getName() << " " << graspSet->getName() <<  "- " << grasp->getName();
+                                ARMARX_CHECK_NOT_NULL(grasp);
+
+                                ARMARX_VERBOSE << node->getName() << " " << graspSet->getName()
+                                               << "- " << grasp->getName();
+
 
-                                const auto global_T_tcp = grasp->getTcpPoseGlobal(node->getGlobalPose());
+                                const auto global_T_tcp =
+                                    grasp->getTcpPoseGlobal(node->getGlobalPose());
 
                                 ARMARX_VERBOSE << "Grasp pose: " << global_T_tcp;
                                 drawGrasp(side, grasp, core::Pose(node->getGlobalPose()), layer);
@@ -374,6 +377,7 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
 
             const auto eefName = arm.getEndEffector();
             const auto eef = robot->getEndEffector(eefName);
+            ARMARX_CHECK_NOT_NULL(eef);
 
             ARMARX_VERBOSE << "preshape: " << grasp->getPreshapeName();
 
@@ -383,29 +387,34 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
             if (preshape != nullptr)
             {
                 jointValues = preshape->getRobotNodeJointValueMap();
-                ARMARX_VERBOSE << "preshape with joint values " << info.jointValues;
+                ARMARX_VERBOSE << "preshape with joint values " << jointValues;
             }
 
+            const auto tcpNode = robotArm.getTCP();
+            ARMARX_CHECK_NOT_NULL(tcpNode);
+            
+            const auto handRootNode = robot->getRobotNode(handRootNodeName);
+            ARMARX_CHECK_NOT_NULL(handRootNode);
+
             ARMARX_TRACE;
             const Eigen::Isometry3f tcp_T_hand_root(
-                robotArm.getTCP()->getPoseInRootFrame().inverse() *
-                robot->getRobotNode(handRootNodeName)->getPoseInRootFrame());
+                tcpNode->getPoseInRootFrame().inverse() *
+                handRootNode->getPoseInRootFrame());
 
-            return Info
-            {
-                .tcp_T_hand_root = tcp_T_hand_root,
-                .jointValues = jointValues,
-                .modelPackagePath = PackagePath(arm.getHandModelPackage(), arm.getHandModelPath())
-            };
+            return Info{.tcp_T_hand_root = tcp_T_hand_root,
+                        .jointValues = jointValues,
+                        .modelPackagePath =
+                            PackagePath(arm.getHandModelPackage(), arm.getHandModelPath())};
         }();
 
         ARMARX_TRACE;
 
-
+        ARMARX_VERBOSE << "Visualizing " << side + "/" + grasp->getName();
         ARMARX_TRACE;
         const auto vizElement =
             viz::Robot(side + "/" + grasp->getName())
-                .file(info.modelPackagePath.serialize().package, info.modelPackagePath.serialize().path)
+                .file(info.modelPackagePath.serialize().package,
+                      info.modelPackagePath.serialize().path)
                 .pose(Eigen::Matrix4f{(global_T_tcp * info.tcp_T_hand_root).matrix()})
                 .joints(info.jointValues)
                 .overrideColor(simox::Color::azure());
@@ -420,7 +429,6 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
         // }
     }
 
-
     ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
 } // namespace armarx::manipulation::components::articulated_object_augmented_visualization

source/armarx/manipulation/skills/ReachGrasp.h

@@ -122,7 +122,7 @@ namespace armarx::manipulation::skills
         std::map<std::string, armarx::armem::articulated_object::ArticulatedObject> environment;
 
 
-    private:
+    protected:
         VirtualRobot::RobotPtr object;
 
         VirtualRobot::GraspPtr grasp;