From 976f3a5b6c97c7da4791f699e4e39baf480fe286 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 20 Mar 2023 08:58:42 +0100
Subject: [PATCH] bugfix: using "info (returned value)" inside lambda. Didn't
know that this was even possible
---
.../Component.cpp | 62 +++++++++++--------
.../armarx/manipulation/skills/ReachGrasp.h | 2 +-
2 files changed, 36 insertions(+), 28 deletions(-)
diff --git a/source/armarx/manipulation/components/articulated_object_augmented_visualization/Component.cpp b/source/armarx/manipulation/components/articulated_object_augmented_visualization/Component.cpp
index fc8fe6c3..dc1d5800 100644
--- a/source/armarx/manipulation/components/articulated_object_augmented_visualization/Component.cpp
+++ b/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
diff --git a/source/armarx/manipulation/skills/ReachGrasp.h b/source/armarx/manipulation/skills/ReachGrasp.h
index ebc12308..9d105501 100644
--- a/source/armarx/manipulation/skills/ReachGrasp.h
+++ b/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;
--
GitLab