| Directory: | ./ |
|---|---|
| File: | src/weighed-distance.cc |
| Date: | 2025-03-07 11:10:46 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 10 | 35 | 28.6% |
| Branches: | 4 | 52 | 7.7% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2015, Joseph Mirabel | ||
| 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) | ||
| 3 | // | ||
| 4 | |||
| 5 | // Redistribution and use in source and binary forms, with or without | ||
| 6 | // modification, are permitted provided that the following conditions are | ||
| 7 | // met: | ||
| 8 | // | ||
| 9 | // 1. Redistributions of source code must retain the above copyright | ||
| 10 | // notice, this list of conditions and the following disclaimer. | ||
| 11 | // | ||
| 12 | // 2. Redistributions in binary form must reproduce the above copyright | ||
| 13 | // notice, this list of conditions and the following disclaimer in the | ||
| 14 | // documentation and/or other materials provided with the distribution. | ||
| 15 | // | ||
| 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
| 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
| 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
| 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
| 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
| 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
| 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
| 27 | // DAMAGE. | ||
| 28 | |||
| 29 | #include <boost/serialization/serialization.hpp> | ||
| 30 | #include <boost/serialization/weak_ptr.hpp> | ||
| 31 | #include <hpp/manipulation/device.hh> | ||
| 32 | #include <hpp/manipulation/graph/edge.hh> | ||
| 33 | #include <hpp/manipulation/graph/graph.hh> | ||
| 34 | #include <hpp/manipulation/roadmap-node.hh> | ||
| 35 | #include <hpp/manipulation/serialization.hh> | ||
| 36 | #include <hpp/manipulation/weighed-distance.hh> | ||
| 37 | #include <hpp/pinocchio/serialization.hh> | ||
| 38 | #include <hpp/util/debug.hh> | ||
| 39 | #include <hpp/util/serialization.hh> | ||
| 40 | |||
| 41 | namespace hpp { | ||
| 42 | namespace manipulation { | ||
| 43 | 2 | WeighedDistancePtr_t WeighedDistance::create(const DevicePtr_t& robot, | |
| 44 | const graph::GraphPtr_t& graph) { | ||
| 45 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | WeighedDistance* ptr = new WeighedDistance(robot, graph); |
| 46 | 2 | WeighedDistancePtr_t shPtr(ptr); | |
| 47 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | ptr->init(shPtr); |
| 48 | 2 | return shPtr; | |
| 49 | } | ||
| 50 | |||
| 51 | ✗ | WeighedDistancePtr_t WeighedDistance::createCopy( | |
| 52 | const WeighedDistancePtr_t& distance) { | ||
| 53 | ✗ | WeighedDistance* ptr = new WeighedDistance(*distance); | |
| 54 | ✗ | WeighedDistancePtr_t shPtr(ptr); | |
| 55 | ✗ | ptr->init(shPtr); | |
| 56 | ✗ | return shPtr; | |
| 57 | } | ||
| 58 | |||
| 59 | ✗ | core::DistancePtr_t WeighedDistance::clone() const { | |
| 60 | ✗ | return createCopy(weak_.lock()); | |
| 61 | } | ||
| 62 | |||
| 63 | 2 | WeighedDistance::WeighedDistance(const DevicePtr_t& robot, | |
| 64 | 2 | const graph::GraphPtr_t graph) | |
| 65 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | : core::WeighedDistance(robot), graph_(graph) {} |
| 66 | |||
| 67 | ✗ | WeighedDistance::WeighedDistance(const WeighedDistance& distance) | |
| 68 | ✗ | : core::WeighedDistance(distance), graph_(distance.graph_) {} | |
| 69 | |||
| 70 | 2 | void WeighedDistance::init(WeighedDistanceWkPtr_t self) { weak_ = self; } | |
| 71 | |||
| 72 | ✗ | value_type WeighedDistance::impl_distance(ConfigurationIn_t q1, | |
| 73 | ConfigurationIn_t q2) const { | ||
| 74 | ✗ | value_type d = core::WeighedDistance::impl_distance(q1, q2); | |
| 75 | ✗ | return d; | |
| 76 | |||
| 77 | // graph::Edges_t pes = graph_->getEdges | ||
| 78 | // (graph_->getNode (q1), graph_->getNode (q2)); | ||
| 79 | // while (!pes.empty ()) { | ||
| 80 | // if (pes.back ()->canConnect (q1, q2)) | ||
| 81 | // return d; | ||
| 82 | // pes.pop_back (); | ||
| 83 | // } | ||
| 84 | // return d + 100; | ||
| 85 | } | ||
| 86 | |||
| 87 | ✗ | value_type WeighedDistance::impl_distance(core::NodePtr_t n1, | |
| 88 | core::NodePtr_t n2) const { | ||
| 89 | ✗ | const Configuration_t &q1 = n1->configuration(), q2 = n2->configuration(); | |
| 90 | ✗ | value_type d = core::WeighedDistance::impl_distance(q1, q2); | |
| 91 | |||
| 92 | graph::Edges_t pes = | ||
| 93 | ✗ | graph_->getEdges(graph_->getState(static_cast<RoadmapNodePtr_t>(n1)), | |
| 94 | ✗ | graph_->getState(static_cast<RoadmapNodePtr_t>(n2))); | |
| 95 | ✗ | while (!pes.empty()) { | |
| 96 | ✗ | if (pes.back()->canConnect(q1, q2)) return d; | |
| 97 | ✗ | pes.pop_back(); | |
| 98 | } | ||
| 99 | ✗ | return d + 100; | |
| 100 | } | ||
| 101 | |||
| 102 | template <typename Archive> | ||
| 103 | ✗ | inline void WeighedDistance::serialize(Archive& ar, | |
| 104 | const unsigned int version) { | ||
| 105 | using namespace boost::serialization; | ||
| 106 | (void)version; | ||
| 107 | ✗ | ar& make_nvp("base", base_object<core::WeighedDistance>(*this)); | |
| 108 | ✗ | ar& BOOST_SERIALIZATION_NVP(graph_); | |
| 109 | ✗ | ar& BOOST_SERIALIZATION_NVP(weak_); | |
| 110 | } | ||
| 111 | |||
| 112 | HPP_SERIALIZATION_IMPLEMENT(WeighedDistance); | ||
| 113 | |||
| 114 | } // namespace manipulation | ||
| 115 | } // namespace hpp | ||
| 116 | |||
| 117 | 1 | BOOST_CLASS_EXPORT(hpp::manipulation::WeighedDistance) | |
| 118 |