From fdf8916464da20c1fe3afe4b4eff19586a94d457 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 12 Sep 2019 09:21:27 +0200
Subject: [PATCH] Add section comments, move methods. Make interpolate()
private.
---
.../RobotState/RobotStateComponent.cpp | 11 +++---
.../RobotState/RobotStateComponent.h | 34 +++++++++++++------
2 files changed, 29 insertions(+), 16 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index d8a78efdd..dd9467673 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -71,6 +71,11 @@ namespace armarx
}
+ std::string RobotStateComponent::getDefaultName() const
+ {
+ return "RobotStateComponent";
+ }
+
void RobotStateComponent::onInitComponent()
{
@@ -264,6 +269,7 @@ namespace armarx
return config;
}
+
bool RobotStateComponent::interpolate(double time, RobotStateConfig& config) const
{
bool success = interpolateJoints(time, config.jointMap);
@@ -587,11 +593,6 @@ namespace armarx
getConfigIdentifier()));
}
- std::string RobotStateComponent::getDefaultName() const
- {
- return "RobotStateComponent";
- }
-
void RobotStateComponent::setRobotStateObserver(RobotStateObserverPtr observer)
{
robotStateObs = observer;
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 162b5c666..baa4426f4 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -79,6 +79,11 @@ namespace armarx
{
public:
+ std::string getDefaultName() const override;
+
+
+ // RobotStateComponentInterface interface
+
/// \return SharedRobotInterface proxy to the internal synchronized robot.
SharedRobotInterfacePrx getSynchronizedRobot(const Ice::Current&) const override;
@@ -90,14 +95,14 @@ namespace armarx
SharedRobotInterfacePrx getRobotSnapshot(const std::string& time, const Ice::Current&) override;
SharedRobotInterfacePrx getRobotSnapshotAtTimestamp(double time, const Ice::Current& current) override;
- NameValueMap getJointConfigAtTimestamp(double, const Ice::Current&) const override;
+ NameValueMap getJointConfigAtTimestamp(double timestamp, const Ice::Current&) const override;
RobotStateConfig getRobotStateAtTimestamp(double timestamp, const Ice::Current&) const override;
/// \return the robot xml filename as specified in the configuration
std::string getRobotFilename(const Ice::Current&) const override;
/// \return All dependent packages, which might contain a robot file.
- std::vector< std::string > getArmarXPackages(const Ice::Current&) const override;
+ std::vector<std::string> getArmarXPackages(const Ice::Current&) const override;
/// \return The name of this robot instance.
std::string getRobotName(const Ice::Current&) const override;
@@ -107,15 +112,6 @@ namespace armarx
/// \return The name of this robot instance.
std::string getRobotNodeSetName(const Ice::Current&) const override;
- /// Create an instance of RobotStatePropertyDefinitions.
- PropertyDefinitionsPtr createPropertyDefinitions() override;
-
- std::string getDefaultName() const override;
-
- void setRobotStateObserver(RobotStateObserverPtr observer);
-
- bool interpolate(double time, RobotStateConfig& config) const;
-
float getScaling(const Ice::Current&) const override;
RobotInfoNodePtr getRobotInfo(const Ice::Current&) const override;
@@ -132,8 +128,14 @@ namespace armarx
void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current& = Ice::emptyCurrent) override;
+ // Own interface.
+ void setRobotStateObserver(RobotStateObserverPtr observer);
+
+
protected:
+ // Component interface.
+
/**
* Load and create a VirtualRobot::Robot instance from the RobotFileName
* property. Additionally listen on the KinematicUnit topic for the
@@ -147,8 +149,12 @@ namespace armarx
/// Calls unref() on RobotStateComponent::_synchronizedPrx.
~RobotStateComponent() override;
+ /// Create an instance of RobotStatePropertyDefinitions.
+ PropertyDefinitionsPtr createPropertyDefinitions() override;
+
// Inherited from KinematicUnitInterface
+
/// Does nothing.
void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
/// Stores the reported joint angles in the joint history and publishes the new joint angles.
@@ -172,10 +178,16 @@ namespace armarx
void readRobotInfo(const std::string& robotFile);
RobotInfoNodePtr readRobotInfo(const RapidXmlReaderNode& infoNode);
+ /// Interpolate the robot state from histories and store it in `config`.
+ bool interpolate(double time, RobotStateConfig& config) const;
+ /// Interpolate the joint angles from history and store it in `jointAngles`.
bool interpolateJoints(double time, NameValueMap& jointAngles) const;
+ /// Interpolate the robot pose from history and store it in `pose`.
bool interpolatePose(double time, FramedPose& pose) const;
+ private:
+
SharedRobotInterfacePrx _synchronizedPrx;
SharedRobotServantPtr _sharedRobotServant;
VirtualRobot::RobotPtr _synchronized;
--
GitLab