From ea98ee00e4cada2596667fca37686e4ae9acc337 Mon Sep 17 00:00:00 2001
From: armar-user <armar@user>
Date: Sat, 20 Feb 2016 15:09:02 +0100
Subject: [PATCH] fixed reconnect in tcp control unit
---
source/RobotAPI/components/units/TCPControlUnit.cpp | 12 ++++++------
source/RobotAPI/components/units/TCPControlUnit.h | 2 +-
2 files changed, 7 insertions(+), 7 deletions(-)
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index ccc88e992..f21bd0df2 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -74,14 +74,14 @@ namespace armarx
//remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
-
+ robotName = robotStateComponentPrx->getRobotName();
localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
jointExistenceCheckRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
localReportRobot = localRobot->clone(localRobot->getName());
std::string nodesetsString = getProperty<std::string>("TCPsToReport").getValue();
-
+ nodesToReport.clear();
if (!nodesetsString.empty())
{
if (nodesetsString == "*")
@@ -1356,7 +1356,7 @@ void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles,
RobotNodePtr& node = nodesToReport.at(i);
const std::string& tcpName = node->getName();
const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
- tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, localReportRobot->getName());
+ tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robotName);
}
@@ -1376,7 +1376,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
if (!localVelReportRobot)
{
- localVelReportRobot = localReportRobot->clone(localReportRobot->getName());
+ localVelReportRobot = localReportRobot->clone(robotName);
}
// ARMARX_DEBUG << jointVel; FramedPoseBaseMap tcpPoses;
@@ -1391,7 +1391,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
RobotNodePtr node = nodesToReport.at(i);
const std::string& tcpName = node->getName();
const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
- tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, localReportRobot->getName());
+ tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robotName);
}
@@ -1427,7 +1427,7 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
// Eigen::AngleAxisf orient(rot.block<3,3>(0,0));
- tcpOrientationVelocities[tcpName] = new FramedDirection(rpy / tDelta, rootFrame, localReportRobot->getName());
+ tcpOrientationVelocities[tcpName] = new FramedDirection(rpy / tDelta, rootFrame, robotName);
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index 7611446f3..10d8bde11 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -194,7 +194,7 @@ namespace armarx
PeriodicTask<TCPControlUnit>::pointer_type execTask;
PeriodicTask<TCPControlUnit>::pointer_type topicTask;
-
+ std::string robotName;
RobotStateComponentInterfacePrx robotStateComponentPrx;
KinematicUnitInterfacePrx kinematicUnitPrx;
VirtualRobot::RobotPtr jointExistenceCheckRobot;
--
GitLab