GCC Code Coverage Report

Directory: ./
File: src/graph/state-selector.cc
Date: 2025-05-07 11:07:45
Exec Total Coverage
Lines: 27 61 44.3%
Branches: 14 74 18.9%
Line Branch Exec Source
1 // Copyright (c) 2014, LAAS-CNRS
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 "hpp/manipulation/graph/state-selector.hh"
30
31 #include <cstdlib>
32 #include <hpp/core/node.hh>
33 #include <hpp/pinocchio/configuration.hh>
34
35 #include "hpp/manipulation/roadmap-node.hh"
36
37 namespace hpp {
38 namespace manipulation {
39 namespace graph {
40 4 StateSelectorPtr_t StateSelector::create(const std::string& name) {
41
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 StateSelector* ptr = new StateSelector(name);
42 4 StateSelectorPtr_t shPtr(ptr);
43
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 ptr->init(shPtr);
44 4 return shPtr;
45 }
46
47 4 void StateSelector::init(const StateSelectorPtr_t& weak) { wkPtr_ = weak; }
48
49 4 StatePtr_t StateSelector::createState(const std::string& name, bool waypoint,
50 const int w) {
51 4 StatePtr_t newState = State::create(name);
52 4 newState->stateSelector(wkPtr_);
53
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 newState->parentGraph(graph_);
54 4 newState->isWaypoint(waypoint);
55
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 if (waypoint)
56 ✗ waypoints_.push_back(newState);
57 else {
58 4 bool found = false;
59 4 for (WeighedStates_t::iterator it = orderedStates_.begin();
60
2/2
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 4 times.
 6 it != orderedStates_.end(); ++it) {
61
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (it->first < w) {
62 ✗ orderedStates_.insert(it, WeighedState_t(w, newState));
63 ✗ found = true;
64 ✗ break;
65 }
66 }
67
2/4
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 if (!found) orderedStates_.push_back(WeighedState_t(w, newState));
68 }
69 4 return newState;
70 }
71
72 ✗ States_t StateSelector::getStates() const {
73 ✗ States_t ret;
74 ✗ for (WeighedStates_t::const_iterator it = orderedStates_.begin();
75 ✗ it != orderedStates_.end(); ++it)
76 ✗ ret.push_back(it->second);
77 ✗ return ret;
78 }
79
80 ✗ States_t StateSelector::getWaypointStates() const { return waypoints_; }
81
82 1 StatePtr_t StateSelector::getState(ConfigurationIn_t config) const {
83 1 for (WeighedStates_t::const_iterator it = orderedStates_.begin();
84
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 orderedStates_.end() != it; ++it) {
85
3/6
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 if (it->second->contains(config)) return it->second;
86 }
87 ✗ std::stringstream oss;
88 ✗ oss << "A configuration has no node:" << pinocchio::displayConfig(config);
89 ✗ throw std::logic_error(oss.str());
90 }
91
92 ✗ StatePtr_t StateSelector::getState(RoadmapNodePtr_t node) const {
93 ✗ if (!node->cacheUpToDate()) node->graphState(getState(node->configuration()));
94 ✗ return node->graphState();
95 }
96
97 ✗ EdgePtr_t StateSelector::chooseEdge(RoadmapNodePtr_t from) const {
98 ✗ StatePtr_t state = getState(from);
99 ✗ const Neighbors_t neighborPicker = state->neighbors();
100 ✗ if (neighborPicker.totalWeight() == 0) {
101 ✗ return EdgePtr_t();
102 }
103 ✗ return neighborPicker();
104 }
105
106 ✗ std::ostream& StateSelector::dotPrint(std::ostream& os,
107 dot::DrawingAttributes) const {
108 ✗ for (WeighedStates_t::const_iterator it = orderedStates_.begin();
109 ✗ orderedStates_.end() != it; ++it)
110 ✗ it->second->dotPrint(os);
111 ✗ return os;
112 }
113
114 ✗ std::ostream& StateSelector::print(std::ostream& os) const {
115 ✗ for (WeighedStates_t::const_iterator it = orderedStates_.begin();
116 ✗ orderedStates_.end() != it; ++it)
117 ✗ os << it->first << " " << *it->second;
118 ✗ return os;
119 }
120
121 ✗ GraphPtr_t StateSelector::parentGraph() const { return graph_.lock(); }
122
123 4 void StateSelector::parentGraph(const GraphWkPtr_t& parent) {
124 4 graph_ = parent;
125 4 GraphPtr_t g = graph_.lock();
126
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
 4 assert(g);
127 4 }
128
129 } // namespace graph
130 } // namespace manipulation
131 } // namespace hpp
132