From 0cefa8d9e3aef22810953d92e4d3ec3ea963453a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mirko=20W=C3=A4chter?= <mirko.waechter@kit.edu>
Date: Mon, 22 Jul 2019 10:45:03 +0200
Subject: [PATCH] extensions for multi robot setup
---
.../components/RobotState/RobotStateComponent.cpp | 2 +-
.../components/RobotState/RobotStateComponent.h | 2 ++
.../RobotAPI/components/units/KinematicUnit.cpp | 2 +-
source/RobotAPI/components/units/KinematicUnit.h | 1 +
.../components/units/KinematicUnitObserver.cpp | 2 +-
.../components/units/KinematicUnitObserver.h | 2 ++
.../RobotUnitModules/RobotUnitModuleRobotData.cpp | 8 ++++++++
.../RobotUnitModules/RobotUnitModuleRobotData.h | 15 ++++++++++++++-
.../RobotUnitModules/RobotUnitModuleUnits.cpp | 4 +++-
.../RobotUnitModules/RobotUnitModuleUnits.h | 4 ++++
10 files changed, 37 insertions(+), 5 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 697a5c50c..de0dde42b 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -116,7 +116,7 @@ namespace armarx
{
ARMARX_VERBOSE << "Node: " << node->getName() << endl;
}
- usingTopic(robotNodeSetName + "State");
+ usingTopic(getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State");
try
{
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 10f5224dc..661b9d337 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -57,6 +57,8 @@ namespace armarx
defineOptionalProperty<std::string>("RobotStateReportingTopic", "RobotStateUpdates", "Name of the topic on which updates of the robot state are reported.");
defineOptionalProperty<int>("HistoryLength", 10000, "Number of entries in the robot state history");
defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
+ defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
+
}
};
diff --git a/source/RobotAPI/components/units/KinematicUnit.cpp b/source/RobotAPI/components/units/KinematicUnit.cpp
index 085b96d83..0348c5914 100644
--- a/source/RobotAPI/components/units/KinematicUnit.cpp
+++ b/source/RobotAPI/components/units/KinematicUnit.cpp
@@ -99,7 +99,7 @@ void KinematicUnit::onInitComponent()
robotNodes = robotNodeSetPtr->getAllRobotNodes();
// component dependencies
- listenerName = robotNodeSetName + "State";
+ listenerName = getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State";
offeringTopic(listenerName);
this->onInitKinematicUnit();
diff --git a/source/RobotAPI/components/units/KinematicUnit.h b/source/RobotAPI/components/units/KinematicUnit.h
index 40be0bf28..67b896fc0 100644
--- a/source/RobotAPI/components/units/KinematicUnit.h
+++ b/source/RobotAPI/components/units/KinematicUnit.h
@@ -51,6 +51,7 @@ namespace armarx
defineRequiredProperty<std::string>("RobotNodeSetName", "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
defineOptionalProperty<std::string>("RobotFileNameProject", "", "Project in which the robot filename is located (if robot is loaded from an external project)");
+ defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
}
};
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index 3f8f026aa..ea9b8da61 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -56,7 +56,7 @@ void KinematicUnitObserver::onInitObserver()
offerConditionCheck("larger", new ConditionCheckLarger());
offerConditionCheck("smaller", new ConditionCheckSmaller());
- usingTopic(robotNodeSetName + "State");
+ usingTopic(getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State");
}
void KinematicUnitObserver::onConnectObserver()
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.h b/source/RobotAPI/components/units/KinematicUnitObserver.h
index 43eb7c0f8..92cf7f720 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.h
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.h
@@ -48,6 +48,8 @@ namespace armarx
defineRequiredProperty<std::string>("RobotNodeSetName", "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
defineOptionalProperty<std::string>("RobotFileNameProject", "", "Project in which the robot filename is located (if robot is loaded from an external project)");
+ defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
+
}
};
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index 94c7b9d84..3be94c7a5 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -41,6 +41,11 @@ namespace armarx
return robotPlatformName;
}
+ std::string RobotData::getRobotPlatformInstanceName() const
+ {
+ return robotPlatformInstanceName;
+ }
+
const std::string& RobotData::getRobotNodetSeName() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -82,6 +87,8 @@ namespace armarx
return clone;
}
+
+
void RobotData::_initVirtualRobot()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -94,6 +101,7 @@ namespace armarx
robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
robotFileName = getProperty<std::string>("RobotFileName").getValue();
robotPlatformName = getProperty<std::string>("PlatformName").getValue();
+ robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
//load robot
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
index 0c0457b99..ff1e06379 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
@@ -55,7 +55,11 @@ namespace armarx
"Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
defineOptionalProperty<std::string>(
"PlatformName", "Platform",
- "Name of the platform (will publish values on PlatformName + 'State')");
+ "Name of the platform needs to correspond to a node in the virtual robot.");
+ defineOptionalProperty<std::string>(
+ "PlatformInstanceName", "Platform",
+ "Name of the platform instance (will publish values on PlatformInstanceName + 'State')");
+
}
};
@@ -114,6 +118,12 @@ namespace armarx
*/
std::string getRobotName() const;
+ /**
+ * @brief Returns the name of the robot platform instance. Used for the platform topic: RobotPlatformInstance + "State"
+ */
+ std::string getRobotPlatformInstanceName() const;
+
+
/**
* @brief Returns a clone of the robot's model
* @return A clone of the robot's model
@@ -122,6 +132,7 @@ namespace armarx
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
+
private:
/// @brief The name of the robot's RobotNodeSet
std::string robotNodeSetName;
@@ -131,6 +142,8 @@ namespace armarx
std::string robotFileName;
/// @brief The name of the robot's platform
std::string robotPlatformName;
+ /// @brief The name of the robot's platform instance
+ std::string robotPlatformInstanceName;
/// @brief The robot's model.
VirtualRobot::RobotPtr robot;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index be1150801..59a461de6 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -266,6 +266,8 @@ namespace armarx
properties->setProperty(confPre + "RobotNodeSetName", _module<RobotData>().getRobotNodetSeName());
properties->setProperty(confPre + "RobotFileName", _module<RobotData>().getRobotFileName());
properties->setProperty(confPre + "RobotFileNameProject", _module<RobotData>().getRobotProjectName());
+ properties->setProperty(confPre + "TopicPrefix", getProperty<std::string>("KinematicUnitNameTopicPrefix"));
+
ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
@@ -317,7 +319,7 @@ namespace armarx
Ice::PropertiesPtr properties = getIceProperties()->clone();
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
- properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformName());
+ properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformInstanceName());
ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
//config
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
index 7978f56c7..54d6c2dcc 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
@@ -39,6 +39,10 @@ namespace armarx
defineOptionalProperty<std::string>(
"KinematicUnitName", "KinematicUnit",
"The name of the created kinematic unit");
+ defineOptionalProperty<std::string>(
+ "KinematicUnitNameTopicPrefix", "",
+ "Prefix for the kinematic sensor values topic");
+
defineOptionalProperty<std::string>(
"PlatformUnitName", "PlatformUnit",
--
GitLab