diff --git a/VirtualRobot/Obstacle.cpp b/VirtualRobot/Obstacle.cpp
index ab1601e205b4c1bdea976f6fc6de85685cc38b0c..6df92ebece934a395fbe63e502f4bce1dd7a06cb 100644
--- a/VirtualRobot/Obstacle.cpp
+++ b/VirtualRobot/Obstacle.cpp
@@ -270,20 +270,20 @@ namespace VirtualRobot
}
}
- Obstacle* Obstacle::_clone(const std::string& name, CollisionCheckerPtr colChecker) const
+ Obstacle* Obstacle::_clone(const std::string& name, CollisionCheckerPtr colChecker, float scaling) const
{
VisualizationNodePtr clonedVisualizationNode;
if (visualizationModel)
{
- clonedVisualizationNode = visualizationModel->clone();
+ clonedVisualizationNode = visualizationModel->clone(true, scaling);
}
CollisionModelPtr clonedCollisionModel;
if (collisionModel)
{
- clonedCollisionModel = collisionModel->clone(colChecker);
+ clonedCollisionModel = collisionModel->clone(colChecker, scaling);
}
Obstacle* result = new Obstacle(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker);
diff --git a/VirtualRobot/Obstacle.h b/VirtualRobot/Obstacle.h
index 0d84e94ce824f49bc0870c148e22b096b3b11718..81934a2b56f9c7a268e87277cae02ae3732f8f6d 100644
--- a/VirtualRobot/Obstacle.h
+++ b/VirtualRobot/Obstacle.h
@@ -104,7 +104,7 @@ namespace VirtualRobot
protected:
- virtual Obstacle* _clone(const std::string& name, CollisionCheckerPtr colChecker = CollisionCheckerPtr()) const;
+ virtual Obstacle* _clone(const std::string& name, CollisionCheckerPtr colChecker = CollisionCheckerPtr(), float scaling = 1.0f) const;
std::string filename;
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index 70fea8e20f2c631bc7dd5726d57021360fbee743..67b8b73c95d12f8619029f6dfdf7e8e0e52cf5af 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -360,7 +360,7 @@ namespace VirtualRobot
return type;
}
- void Robot::print()
+ void Robot::print(bool printChildren, bool printDecoration) const
{
cout << "******** Robot ********" << endl;
cout << "* Name: " << name << endl;
@@ -409,7 +409,7 @@ namespace VirtualRobot
/**
* This method returns a reference to Robot::rootNode;
*/
- RobotNodePtr LocalRobot::getRootNode()
+ RobotNodePtr LocalRobot::getRootNode() const
{
return this->rootNode;
}
@@ -491,22 +491,31 @@ namespace VirtualRobot
}
}
- RobotNodeSetPtr LocalRobot::getRobotNodeSet(const std::string& nodeSetName)
+ boost::shared_ptr<Robot> Robot::shared_from_this() const
{
- if (robotNodeSetMap.find(nodeSetName) == robotNodeSetMap.end())
+ auto sceneObject = SceneObject::shared_from_this();
+ boost::shared_ptr<const Robot> robotPtr = boost::static_pointer_cast<const Robot>(sceneObject);
+ boost::shared_ptr<Robot> result = boost::const_pointer_cast<Robot>(robotPtr);
+ return result;
+ }
+
+ RobotNodeSetPtr LocalRobot::getRobotNodeSet(const std::string& nodeSetName) const
+ {
+ auto it = robotNodeSetMap.find(nodeSetName);
+ if (it == robotNodeSetMap.end())
{
VR_WARNING << "No robot node set with name <" << nodeSetName << "> defined." << endl;
return RobotNodeSetPtr();
}
- return robotNodeSetMap[nodeSetName];
+ return it->second;
}
/**
* 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::getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSets)
+ void LocalRobot::getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSets) const
{
storeNodeSets.clear();
storeNodeSets.reserve(robotNodeSetMap.size());
@@ -519,7 +528,7 @@ namespace VirtualRobot
}
}
- std::vector<RobotNodeSetPtr> Robot::getRobotNodeSets()
+ std::vector<RobotNodeSetPtr> Robot::getRobotNodeSets() const
{
std::vector<RobotNodeSetPtr> res;
getRobotNodeSets(res);
@@ -605,6 +614,11 @@ namespace VirtualRobot
return res;
}
+ void Robot::setGlobalPose(const Eigen::Matrix4f &globalPose)
+ {
+ setGlobalPose(globalPose, true);
+ }
+
VirtualRobot::CollisionCheckerPtr Robot::getCollisionChecker()
{
std::vector<RobotNodePtr> robotNodes = this->getRobotNodes();
@@ -633,7 +647,7 @@ namespace VirtualRobot
}
}
- Eigen::Matrix4f LocalRobot::getGlobalPose()
+ Eigen::Matrix4f LocalRobot::getGlobalPose() const
{
ReadLock(mutex, use_mutex);
return globalPose;
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index fc26c92c1e032f21dd1bc0352c1e2e3d867292fc..08fc8a28725eba78a5945d07a0bb6a3186f05147 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -70,7 +70,7 @@ namespace VirtualRobot
The root node is the first RobotNode of this robot.
*/
virtual void setRootNode(RobotNodePtr node) = 0;
- virtual RobotNodePtr getRootNode() = 0;
+ virtual RobotNodePtr getRootNode() const = 0;
virtual void applyJointValues();
@@ -126,7 +126,7 @@ namespace VirtualRobot
/*!
Print status information.
*/
- virtual void print();
+ virtual void print(bool printChildren = false, bool printDecoration = true) const;
/*!
Enables/Disables the visualization updates of collision model and visualization model.
@@ -134,10 +134,7 @@ namespace VirtualRobot
void setUpdateVisualization(bool enable);
void setUpdateCollisionModel(bool enable);
- boost::shared_ptr<Robot> shared_from_this()
- {
- return boost::static_pointer_cast<Robot>(SceneObject::shared_from_this());
- }
+ boost::shared_ptr<Robot> shared_from_this() const;
//boost::shared_ptr<Robot> shared_from_this() const { return boost::static_pointer_cast<Robot>(SceneObject::shared_from_this()); }
/*!
@@ -167,9 +164,9 @@ namespace VirtualRobot
virtual void deregisterRobotNodeSet(RobotNodeSetPtr nodeSet) = 0;
virtual bool hasRobotNodeSet(RobotNodeSetPtr nodeSet);
virtual bool hasRobotNodeSet(const std::string& name) = 0;
- virtual RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) = 0;
- virtual std::vector<RobotNodeSetPtr> getRobotNodeSets();
- virtual void getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSet) = 0;
+ virtual RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const = 0;
+ virtual std::vector<RobotNodeSetPtr> getRobotNodeSets() const;
+ virtual void getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSet) const = 0;
/**
*
@@ -203,7 +200,8 @@ namespace VirtualRobot
/*!
Set the global position of this robot
*/
- virtual void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues = true) = 0;
+ virtual void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues) = 0;
+ void setGlobalPose(const Eigen::Matrix4f& globalPose);
/*!
Set the global pose of this robot so that the RobotNode node is at position globalPoseNode
@@ -398,7 +396,7 @@ namespace VirtualRobot
bool updateVisualization;
- boost::recursive_mutex mutex;
+ mutable boost::recursive_mutex mutex;
bool use_mutex;
};
@@ -444,7 +442,7 @@ namespace VirtualRobot
virtual ~LocalRobot();
virtual void setRootNode(RobotNodePtr node);
- virtual RobotNodePtr getRootNode();
+ virtual RobotNodePtr getRootNode() const;
virtual void registerRobotNode(RobotNodePtr node);
virtual void deregisterRobotNode(RobotNodePtr node);
@@ -456,8 +454,8 @@ namespace VirtualRobot
virtual void registerRobotNodeSet(RobotNodeSetPtr nodeSet);
virtual void deregisterRobotNodeSet(RobotNodeSetPtr nodeSet);
virtual bool hasRobotNodeSet(const std::string& name);
- virtual RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName);
- virtual void getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSet);
+ virtual RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const;
+ virtual void getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSet) const;
virtual void registerEndEffector(EndEffectorPtr endEffector);
virtual bool hasEndEffector(const std::string& endEffectorName);
@@ -466,7 +464,7 @@ namespace VirtualRobot
virtual void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyJointValues = true);
virtual void setGlobalPose(const Eigen::Matrix4f& globalPose);
- virtual Eigen::Matrix4f getGlobalPose();
+ virtual Eigen::Matrix4f getGlobalPose() const;
protected:
//Eigen::Matrix4f globalPose; //!< The pose of this robot in the world
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index 76c24bb46d8e458569c042805bbd609c0653c1ab..3c03976193626dbd541437a8540ce8f7dbe70e38 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -215,14 +215,14 @@ namespace VirtualRobot
return true;
}
- void SceneObject::showCoordinateSystem(bool enable, float scaling, std::string* text)
+ void SceneObject::showCoordinateSystem(bool enable, float scaling, std::string* text, const std::string &visualizationType)
{
if (!enable && !visualizationModel)
{
return; // nothing to do
}
- if (!ensureVisualization())
+ if (!ensureVisualization(visualizationType))
{
return;
}
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index e9afe168c1751f6ad1c3bfc0a9ead3637c387a02..9db3f4d3e61a803509c889256ca1cf91a3c5364b 100644
--- a/VirtualRobot/SceneObject.h
+++ b/VirtualRobot/SceneObject.h
@@ -166,7 +166,7 @@ namespace VirtualRobot
\p scaling Size of coordinate system
\p text Text to display at coordinate system. If not given, the name of this robot node will be displayed.
*/
- virtual void showCoordinateSystem(bool enable, float scaling = 1.0f, std::string* text = NULL);
+ virtual void showCoordinateSystem(bool enable, float scaling = 1.0f, std::string* text = NULL, const std::string& visualizationType = "");
/*!
Display some physics debugging information.