diff --git a/CMakeLists.txt b/CMakeLists.txt
index a054a8d2c8dcf440ddb74c30623317f052f11f5d..70ba1173c3d9632c1e1925556a28821363549397 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -68,6 +68,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/constraint-set.hh
include/hpp/manipulation/roadmap.hh
include/hpp/manipulation/roadmap-node.hh
+ include/hpp/manipulation/connected-component.hh
include/hpp/manipulation/manipulation-planner.hh
include/hpp/manipulation/graph-path-validation.hh
include/hpp/manipulation/graph-steering-method.hh
diff --git a/include/connected-component.hh b/include/hpp/manipulation/connected-component.hh
similarity index 57%
rename from include/connected-component.hh
rename to include/hpp/manipulation/connected-component.hh
index dc3b5391fdfc60ab95c52e8a667ba8ac00bf1cc0..bfacb7d5bd280c1f2af64cee5b4c6bed5a8f58f1 100644
--- a/include/connected-component.hh
+++ b/include/hpp/manipulation/connected-component.hh
@@ -21,26 +21,39 @@
#include <hpp/core/connected-component.hh>
-# include "hpp/manipulation/fwd.hh"
-# include "hpp/manipulation/graph/fwd.hh"
+# include "hpp/manipulation/roadmap.hh"
+# include "hpp/manipulation/roadmap-node.hh"
namespace hpp {
namespace manipulation {
- /// \addtogroup connected-component
- /// \{
/// Extension of hpp::core::connected-component. Adds a list of roadmap nodes for
/// every contraint graph node within the connected component. Thus every roadmap
/// node is assigned to a grahp node, which minimises computation time.
class HPP_MANIPULATION_DLLAPI ConnectedComponent : public core::ConnectedComponent
{
public:
- // Map of graph nodes within the connected component
- typedef std::map <char, graph::NodePtr_t> graphNodes_t;
+ typedef std::vector<RoadmapNodePtr_t> RoadmapNodes_t;
+ /// Map of graph nodes within the connected component
+ typedef std::map <graph::NodePtr_t, RoadmapNodes_t> GraphNodes_t;
+ typedef boost::shared_ptr<ConnectedComponent> ManipulationConnectedComponentPtr_t;
+
+ /// return a shared pointer to new instance of manipulation::ConnectedComponent
+ static ManipulationConnectedComponentPtr_t create (const RoadmapWkPtr_t& Roadmap);
+
+ /// Merge two connected components (extension of core::ConnectedComponent::merge)
+ /// \param other manipulation connected component to merge into this one.
+ /// \note other will be empty after calling this method.
+ void merge (const ManipulationConnectedComponentPtr_t& other);
+
+ /// Add roadmap node to connected component
+ /// \param roadmap node to be added
+ void addNode (const RoadmapNodePtr_t& node);
+
protected:
private:
- std:
+ GraphNodes_t GraphNodeMap_;
+ static RoadmapPtr_t Roadmap_;
}; // class ConnectedComponent
- /// \}
} // namespace manipulation
} // namespace hpp
#endif // HPP_MANIPULATION_CONNECTED_COMPONENT_HH
diff --git a/include/hpp/manipulation/roadmap.hh b/include/hpp/manipulation/roadmap.hh
index df3582cdbee7d792fb1d43d43f7519d46a94c3b1..f9f0b01da309dea1bc108985ead3f80a7a566e88 100644
--- a/include/hpp/manipulation/roadmap.hh
+++ b/include/hpp/manipulation/roadmap.hh
@@ -62,6 +62,9 @@ namespace hpp {
const graph::NodePtr_t& node,
value_type& minDistance) const;
+ /// Get graph node corresponding to given roadmap node
+ graph::NodePtr_t getNode(RoadmapNodePtr_t node);
+
protected:
/// Register a new configuration.
void statInsert (const RoadmapNodePtr_t& n);
@@ -72,12 +75,18 @@ namespace hpp {
/// Node factory
core::NodePtr_t createNode (const ConfigurationPtr_t& config) const;
+ void init (const RoadmapPtr_t& shPtr)
+ {
+ weak_ = shPtr;
+ }
+
private:
typedef std::list < graph::HistogramPtr_t > Histograms;
/// Keep track of the leaf that are explored.
/// There should be one histogram per foliation.
Histograms histograms_;
graph::GraphPtr_t graph_;
+ RoadmapWkPtr_t weak_;
};
/// \}
} // namespace manipulation
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f5920d83a9f4aeecdc332ef17b73f18e7ec52cb3..c8ba7f57a91928bcd237a8412ceb921f83d05825 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -25,6 +25,7 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
manipulation-planner.cc
problem-solver.cc
roadmap.cc
+ connected-component.cc
constraint-set.cc
roadmap-node.cc
device.cc
diff --git a/src/connected-component.cc b/src/connected-component.cc
new file mode 100644
index 0000000000000000000000000000000000000000..700679252d3def781474123a59aeb3c080d570ae
--- /dev/null
+++ b/src/connected-component.cc
@@ -0,0 +1,67 @@
+//
+// Copyright (c) 2015 CNRS
+// Authors: Anna Seppala (seppala@laas.fr)
+//
+// This file is part of hpp-core
+// hpp-core is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-core is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-core If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include <hpp/manipulation/connected-component.hh>
+
+namespace hpp {
+ namespace manipulation {
+
+ ConnectedComponent::ManipulationConnectedComponentPtr_t
+ ConnectedComponent::create(const RoadmapWkPtr_t& Roadmap)
+ {
+ ConnectedComponent* ptr = new ConnectedComponent ();
+ ConnectedComponent::ManipulationConnectedComponentPtr_t shPtr (ptr);
+ // calls init function in core::ConnectedComponent that saves
+ // shPtr into the class variable weak_ (weak pointer). Reimplement?
+ ptr->init (shPtr);
+ Roadmap_ = Roadmap.lock ();
+ return shPtr;
+ }
+
+ void ConnectedComponent::merge (const ManipulationConnectedComponentPtr_t& other)
+ {
+ core::ConnectedComponent::merge(other);
+
+// take all graph nodes in other->GraphNodeMap_ and put them in this->GraphNodeMap_
+// if they already exist in this->GraphNodeMap_, append roadmap nodes from graph node in other
+// to graph node in this.
+
+ other->GraphNodeMap_.clear();
+ }
+
+ void ConnectedComponent::addNode(const RoadmapNodePtr_t& node)
+ {
+ core::ConnectedComponent::addNode(node);
+ // Find right graph node in map and add roadmap node to corresponding vector
+ GraphNodes_t::iterator mapIt = GraphNodeMap_.find(Roadmap_->getNode(node));
+ if (mapIt != GraphNodeMap_.end()) {
+ mapIt->second.push_back(node);
+ // if graph node not found, add new map element with one roadmap node
+ } else {
+ RoadmapNodes_t newRoadmapNodeVector;
+ newRoadmapNodeVector.push_back(node);
+ GraphNodeMap_.insert(std::pair<graph::NodePtr_t, RoadmapNodes_t>
+ (Roadmap_->getNode(node), newRoadmapNodeVector));
+ }
+
+ }
+
+
+ } // namespace manipulation
+} // namespace hpp
+
diff --git a/src/roadmap.cc b/src/roadmap.cc
index 2bc24768495ea70b96d3d9ed08ad8252891aaea8..e83f964786e5f39a2f188d044fa637947e340345 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -20,16 +20,20 @@
#include <hpp/core/distance.hh>
#include <hpp/core/connected-component.hh>
+#include <hpp/manipulation/connected-component.hh>
#include <hpp/manipulation/roadmap-node.hh>
namespace hpp {
namespace manipulation {
Roadmap::Roadmap (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot) :
- core::Roadmap (distance, robot) {}
+ core::Roadmap (distance, robot), weak_ () {}
RoadmapPtr_t Roadmap::create (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot)
{
- return RoadmapPtr_t (new Roadmap (distance, robot));
+ Roadmap* ptr = new Roadmap (distance, robot);
+ RoadmapPtr_t shPtr (ptr);
+ ptr->init(shPtr);
+ return shPtr;
}
void Roadmap::clear ()
@@ -104,7 +108,14 @@ namespace hpp {
core::NodePtr_t Roadmap::createNode (const ConfigurationPtr_t& q) const
{
- return RoadmapNodePtr_t (new RoadmapNode (q));
+ // call RoadmapNode constructor with new manipulation connected component
+ return RoadmapNodePtr_t (new RoadmapNode (q, ConnectedComponent::create(weak_)));
}
+
+ graph::NodePtr_t Roadmap::getNode(RoadmapNodePtr_t node)
+ {
+ return graph_->getNode(node);
+ }
+
} // namespace manipulation
} // namespace hpp