diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index bbe8d053d871e641b68ce3db7ec52a4e799d75a5..bc5f88c866919ba58f167387bd89cea2b3b679da 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -2,6 +2,7 @@
#include "Robot.h"
#include "RobotConfig.h"
#include "Trajectory.h"
+#include "VirtualRobot.h"
#include "VirtualRobotException.h"
#include "Nodes/Sensor.h"
#include "Visualization/VisualizationNode.h"
@@ -137,7 +138,7 @@ namespace VirtualRobot
}
}
- bool LocalRobot::hasRobotNode(RobotNodePtr node)
+ bool LocalRobot::hasRobotNode(RobotNodePtr node) const
{
if (!node)
{
@@ -154,7 +155,7 @@ namespace VirtualRobot
return false;
}
- bool LocalRobot::hasRobotNode(const std::string& robotNodeName)
+ bool LocalRobot::hasRobotNode(const std::string& robotNodeName) const
{
return (robotNodeMap.find(robotNodeName) != robotNodeMap.end());
}
@@ -195,7 +196,7 @@ namespace VirtualRobot
robotNodeSetMap[nodeSetName] = nodeSet;
}
- bool Robot::hasRobotNodeSet(RobotNodeSetPtr nodeSet)
+ bool Robot::hasRobotNodeSet(RobotNodeSetPtr nodeSet) const
{
if (!nodeSet)
{
@@ -207,14 +208,9 @@ namespace VirtualRobot
return hasRobotNodeSet(nodeSetName);
}
- bool LocalRobot::hasRobotNodeSet(const std::string& name)
+ bool LocalRobot::hasRobotNodeSet(const std::string& name) const
{
- if (robotNodeSetMap.find(name) != robotNodeSetMap.end())
- {
- return true;
- }
-
- return false;
+ return robotNodeSetMap.find(name) != robotNodeSetMap.end();
}
void LocalRobot::deregisterRobotNodeSet(RobotNodeSetPtr nodeSet)
@@ -263,7 +259,7 @@ namespace VirtualRobot
/**
* \return true if instance of VirtualRobot::Robot contains a reference to \p endEffector and false otherwise
*/
- bool Robot::hasEndEffector(EndEffectorPtr endEffector)
+ bool Robot::hasEndEffector(EndEffectorPtr endEffector) const
{
if (!endEffector)
{
@@ -284,7 +280,7 @@ namespace VirtualRobot
/**
* \return true if instance of VirtualRobot::Robot contains an endeffector with name \p endEffectorName and false otherwise
*/
- bool LocalRobot::hasEndEffector(const std::string& endEffectorName)
+ bool LocalRobot::hasEndEffector(const std::string& endEffectorName) const
{
if (endEffectorName.empty())
{
@@ -303,7 +299,7 @@ namespace VirtualRobot
/**
* \return reference to endeffector with name \p endEffectorName or Null-Pointer otherwise
*/
- EndEffectorPtr LocalRobot::getEndEffector(const std::string& endEffectorName)
+ EndEffectorPtr LocalRobot::getEndEffector(const std::string& endEffectorName) const
{
if (endEffectorMap.find(endEffectorName) == endEffectorMap.end())
{
@@ -311,14 +307,14 @@ namespace VirtualRobot
return EndEffectorPtr();
}
- return endEffectorMap[endEffectorName];
+ return endEffectorMap.at(endEffectorName);
}
/**
* This method stores all endeffectors belonging to the robot in \p storeEEF.
* If there are no registered endeffectors \p storeEEF will be empty.
*/
- void LocalRobot::getEndEffectors(std::vector<EndEffectorPtr>& storeEEF)
+ void LocalRobot::getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) const
{
storeEEF.clear();
storeEEF.reserve(endEffectorMap.size());
@@ -341,7 +337,7 @@ namespace VirtualRobot
}
- std::vector<EndEffectorPtr> Robot::getEndEffectors()
+ std::vector<EndEffectorPtr> Robot::getEndEffectors() const
{
std::vector<EndEffectorPtr> res;
getEndEffectors(res);
@@ -658,7 +654,7 @@ namespace VirtualRobot
{
std::vector<RobotNodePtr> robotNodes = this->getRobotNodes();
- if (robotNodes.size() == 0)
+ if (robotNodes.empty())
{
return CollisionChecker::getGlobalCollisionChecker();
}
@@ -720,7 +716,7 @@ namespace VirtualRobot
return res;
}
- float Robot::getMass()
+ float Robot::getMass() const
{
float res = 0;
std::vector<RobotNodePtr> robotNodes = this->getRobotNodes();
@@ -735,7 +731,7 @@ namespace VirtualRobot
return res;
}
- std::vector< CollisionModelPtr > Robot::getCollisionModels()
+ std::vector< CollisionModelPtr > Robot::getCollisionModels() const
{
std::vector< CollisionModelPtr > result;
std::vector<RobotNodePtr> robotNodes = this->getRobotNodes();
@@ -1106,7 +1102,7 @@ namespace VirtualRobot
applyJointValuesNoLock();
}
- VirtualRobot::BoundingBox Robot::getBoundingBox(bool collisionModel)
+ VirtualRobot::BoundingBox Robot::getBoundingBox(bool collisionModel) const
{
VirtualRobot::BoundingBox bbox;
std::vector<RobotNodePtr> rn = getRobotNodes();
@@ -1126,7 +1122,7 @@ namespace VirtualRobot
return bbox;
}
- SensorPtr Robot::getSensor(const std::string& name)
+ SensorPtr Robot::getSensor(const std::string& name) const
{
std::vector<RobotNodePtr> rn = getRobotNodes();
@@ -1146,7 +1142,7 @@ namespace VirtualRobot
return SensorPtr();
}
- std::vector<SensorPtr> Robot::getSensors()
+ std::vector<SensorPtr> Robot::getSensors() const
{
std::vector<SensorPtr> result;
std::vector<RobotNodePtr> rn = getRobotNodes();
@@ -1155,7 +1151,7 @@ namespace VirtualRobot
{
std::vector<SensorPtr> s = i->getSensors();
- if (s.size() > 0)
+ if (!s.empty())
{
result.insert(result.end(), s.begin(), s.end());
}
@@ -1164,7 +1160,7 @@ namespace VirtualRobot
return result;
}
- std::string Robot::toXML(const std::string& basePath, const std::string& modelPath /*= "models"*/, bool storeEEF, bool storeRNS, bool storeSensors)
+ std::string Robot::toXML(const std::string& basePath, const std::string& modelPath /*= "models"*/, bool storeEEF, bool storeRNS, bool storeSensors) const
{
std::stringstream ss;
ss << "<?xml version='1.0' encoding='UTF-8'?>" << endl << std::endl;
@@ -1215,7 +1211,7 @@ namespace VirtualRobot
return ss.str();
}
- float Robot::getScaling()
+ float Robot::getScaling() const
{
return scaling;
}
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 2f93f655a61d9c3bf292e4dc158ef15062db3920..d9e35d52bc4569228c5069a4a3f8e7e3d1313205 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -148,8 +148,8 @@ namespace VirtualRobot
*/
virtual void registerRobotNode(RobotNodePtr node) = 0;
virtual void deregisterRobotNode(RobotNodePtr node) = 0;
- virtual bool hasRobotNode(RobotNodePtr node) = 0;
- virtual bool hasRobotNode(const std::string& robotNodeName) = 0;
+ virtual bool hasRobotNode(RobotNodePtr node) const = 0;
+ virtual bool hasRobotNode(const std::string& robotNodeName) const = 0;
virtual RobotNodePtr getRobotNode(const std::string& robotNodeName) const = 0;
virtual std::vector< RobotNodePtr > getRobotNodes() const;
virtual std::vector<std::string> getRobotNodeNames() const;
@@ -160,8 +160,8 @@ namespace VirtualRobot
*/
virtual void registerRobotNodeSet(RobotNodeSetPtr nodeSet) = 0;
virtual void deregisterRobotNodeSet(RobotNodeSetPtr nodeSet) = 0;
- virtual bool hasRobotNodeSet(RobotNodeSetPtr nodeSet);
- virtual bool hasRobotNodeSet(const std::string& name) = 0;
+ virtual bool hasRobotNodeSet(RobotNodeSetPtr nodeSet) const;
+ virtual bool hasRobotNodeSet(const std::string& name) const = 0;
virtual RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const = 0;
virtual std::vector<RobotNodeSetPtr> getRobotNodeSets() const;
virtual std::vector<std::string> getRobotNodeSetNames() const;
@@ -171,13 +171,13 @@ namespace VirtualRobot
*
*/
virtual void registerEndEffector(EndEffectorPtr endEffector) = 0;
- virtual bool hasEndEffector(EndEffectorPtr endEffector);
- virtual bool hasEndEffector(const std::string& endEffectorName) = 0;
- virtual EndEffectorPtr getEndEffector(const std::string& endEffectorName) = 0;
- virtual std::vector<EndEffectorPtr> getEndEffectors();
- virtual void getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) = 0;
+ virtual bool hasEndEffector(EndEffectorPtr endEffector) const;
+ virtual bool hasEndEffector(const std::string& endEffectorName) const = 0;
+ virtual EndEffectorPtr getEndEffector(const std::string& endEffectorName) const = 0;
+ virtual std::vector<EndEffectorPtr> getEndEffectors() const;
+ virtual void getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) const = 0;
- virtual std::vector< CollisionModelPtr > getCollisionModels();
+ virtual std::vector< CollisionModelPtr > getCollisionModels() const;
CollisionCheckerPtr getCollisionChecker() override;
@@ -220,7 +220,7 @@ namespace VirtualRobot
Eigen::Vector3f getCoMGlobal() override;
//! Return accumulated mass of this robot.
- virtual float getMass();
+ virtual float getMass() const;
/*!
@@ -367,15 +367,15 @@ namespace VirtualRobot
\param collisionModel Either the collision or the visualization model is considered.
\return The bounding box.
*/
- virtual BoundingBox getBoundingBox(bool collisionModel = true);
+ virtual BoundingBox getBoundingBox(bool collisionModel = true) const;
/*!
* Returns the sensor with the given name.
*/
- virtual SensorPtr getSensor(const std::string& name);
+ virtual SensorPtr getSensor(const std::string& name) const;
template<class SensorType>
- std::shared_ptr<SensorType> getSensor(const std::string& name)
+ std::shared_ptr<SensorType> getSensor(const std::string& name) const
{
return std::dynamic_pointer_cast<SensorType>(getSensor(name));
}
@@ -383,10 +383,10 @@ namespace VirtualRobot
/*!
Returns all sensors that are defined within this robot.
*/
- virtual std::vector<SensorPtr> getSensors();
+ virtual std::vector<SensorPtr> getSensors() const;
template<class SensorType>
- std::vector<std::shared_ptr<SensorType> > getSensors()
+ std::vector<std::shared_ptr<SensorType> > getSensors() const
{
std::vector<std::shared_ptr<SensorType> > result;
std::vector<SensorPtr> sensors = getSensors();
@@ -404,9 +404,9 @@ namespace VirtualRobot
Creates an XML string that defines the complete robot. Filenames of all visualization models are set to modelPath/RobotNodeName_visu and/or modelPath/RobotNodeName_colmodel.
@see RobotIO::saveXML.
*/
- virtual std::string toXML(const std::string& basePath = ".", const std::string& modelPath = "models", bool storeEEF = true, bool storeRNS = true, bool storeSensors = true);
+ virtual std::string toXML(const std::string& basePath = ".", const std::string& modelPath = "models", bool storeEEF = true, bool storeRNS = true, bool storeSensors = true) const;
- float getScaling();
+ float getScaling() const;
void setScaling(float scaling);
/**
@@ -503,21 +503,21 @@ namespace VirtualRobot
void registerRobotNode(RobotNodePtr node) override;
void deregisterRobotNode(RobotNodePtr node) override;
- bool hasRobotNode(const std::string& robotNodeName) override;
- bool hasRobotNode(RobotNodePtr node) override;
+ bool hasRobotNode(const std::string& robotNodeName) const override;
+ bool hasRobotNode(RobotNodePtr node) const override;
RobotNodePtr getRobotNode(const std::string& robotNodeName) const override;
void getRobotNodes(std::vector< RobotNodePtr >& storeNodes, bool clearVector = true) const override;
void registerRobotNodeSet(RobotNodeSetPtr nodeSet) override;
void deregisterRobotNodeSet(RobotNodeSetPtr nodeSet) override;
- bool hasRobotNodeSet(const std::string& name) override;
+ bool hasRobotNodeSet(const std::string& name) const override;
RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const override;
void getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSet) const override;
void registerEndEffector(EndEffectorPtr endEffector) override;
- bool hasEndEffector(const std::string& endEffectorName) override;
- EndEffectorPtr getEndEffector(const std::string& endEffectorName) override;
- void getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) override;
+ bool hasEndEffector(const std::string& endEffectorName) const override;
+ EndEffectorPtr getEndEffector(const std::string& endEffectorName) const override;
+ void getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) const override;
void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyJointValues = true) override;
void setGlobalPose(const Eigen::Matrix4f& globalPose) override;
@@ -531,6 +531,7 @@ namespace VirtualRobot
std::map< std::string, RobotNodePtr > robotNodeMap;
std::map< std::string, RobotNodeSetPtr > robotNodeSetMap;
std::map< std::string, EndEffectorPtr > endEffectorMap;
+
};