From 3b3e9e67d91968851cb3e69a93937d4e56ec2678 Mon Sep 17 00:00:00 2001
From: Patrick Dormanns <patrick.dormanns@student.kit.edu>
Date: Mon, 26 Feb 2024 14:57:10 +0100
Subject: [PATCH] error handling for FramdPosition-conversion
---
source/armarx/control/pointing/core/Pointing.cpp | 14 +++++++++++++-
source/armarx/control/pointing/core/Pointing.h | 2 ++
2 files changed, 15 insertions(+), 1 deletion(-)
diff --git a/source/armarx/control/pointing/core/Pointing.cpp b/source/armarx/control/pointing/core/Pointing.cpp
index e9824dac..2da46f95 100644
--- a/source/armarx/control/pointing/core/Pointing.cpp
+++ b/source/armarx/control/pointing/core/Pointing.cpp
@@ -30,7 +30,7 @@ namespace armarx::control::pointing::core
VirtualRobot::RobotPtr robot = getRobot();
- Eigen::Vector3f target = params.target.toGlobalEigen(robot);
+ Eigen::Vector3f target = toGlobal(params.target, robot);
visualizeTarget(target);
armarx::TrajectoryPtr traj = computeArmTrajectory(robot, params.side, target);
@@ -70,6 +70,18 @@ namespace armarx::control::pointing::core
return robot;
}
+ Eigen::Vector3f
+ Pointing::toGlobal(const FramedPosition& framedPosition, VirtualRobot::RobotPtr robot)
+ {
+ if (not(framedPosition.frame == armarx::GlobalFrame ||
+ robot->hasRobotNode(framedPosition.frame)))
+ {
+ ARMARX_ERROR << "'" << framedPosition.frame << "' is not a valid frame name";
+ throw armarx::exceptions::local::PointingFailedException();
+ }
+ return framedPosition.toGlobalEigen(robot);
+ }
+
void
Pointing::visualizeTarget(const Eigen::Vector3f& target)
{
diff --git a/source/armarx/control/pointing/core/Pointing.h b/source/armarx/control/pointing/core/Pointing.h
index c36d9b88..3e035804 100644
--- a/source/armarx/control/pointing/core/Pointing.h
+++ b/source/armarx/control/pointing/core/Pointing.h
@@ -92,6 +92,8 @@ namespace armarx::control::pointing::core
private:
VirtualRobot::RobotPtr getRobot();
+ Eigen::Vector3f toGlobal(const armarx::FramedPosition&, VirtualRobot::RobotPtr);
+
void visualizeTarget(const Eigen::Vector3f& target);
armarx::TrajectoryPtr
--
GitLab