1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Willow Garage
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35 /* Author: Ioan Sucan */
36
37 #define BOOST_TEST_MODULE "StateStorage"
38 #include <boost/test/unit_test.hpp>
39 #include "ompl/base/StateStorage.h"
40 #include "ompl/base/ScopedState.h"
41 #include "ompl/base/spaces/SE3StateSpace.h"
42 #include "ompl/base/spaces/SE2StateSpace.h"
43
44 using namespace ompl;
45
46 // define a convenience macro
47 #define BOOST_OMPL_EXPECT_NEAR(a, b, diff) BOOST_CHECK_SMALL((a) - (b), diff)
48
49 struct Metadata
50 {
51 Metadata() = default;
52
53 int tag1{0};
54 float tag2{0.5f};
55
56 template<typename Archive>
serializeMetadata57 void serialize(Archive & ar, const unsigned int /*version*/)
58 {
59 ar & tag1;
60 ar & tag2;
61 }
62 };
63
64
BOOST_AUTO_TEST_CASE(Store)65 BOOST_AUTO_TEST_CASE(Store)
66 {
67 auto space(std::make_shared<base::SE3StateSpace>());
68 base::RealVectorBounds bounds(3);
69 bounds.setLow(-1);
70 bounds.setHigh(1);
71 space->setBounds(bounds);
72 space->setup();
73
74 base::StateStorage ss(space);
75 base::ScopedState<> s(space);
76 base::State *x = space->allocState();
77 for (int i = 0 ; i < 1000 ; ++i)
78 {
79 s.random();
80 space->copyState(x, s.get());
81 ss.addState(x);
82 }
83 space->freeState(x);
84 ss.store("tmp_states");
85
86 base::StateStorageWithMetadata<Metadata> ssm(space);
87 ssm.generateSamples(3);
88 ssm.getMetadata(0).tag1 = 2;
89 ssm.getMetadata(1).tag2 = 1.0f;
90 ssm.store("tmp_states_wm");
91 }
92
BOOST_AUTO_TEST_CASE(Load)93 BOOST_AUTO_TEST_CASE(Load)
94 {
95 auto space(std::make_shared<base::SE3StateSpace>());
96 base::RealVectorBounds bounds(3);
97 bounds.setLow(-1);
98 bounds.setHigh(1);
99 space->setBounds(bounds);
100 space->setup();
101
102 base::StateStorage ss(space);
103 ss.load("tmp_states");
104
105 base::StateStorageWithMetadata<Metadata> ssm(space);
106 ssm.load("tmp_states_wm");
107 BOOST_CHECK_EQUAL(ssm.getMetadata(0).tag1, 2);
108 BOOST_OMPL_EXPECT_NEAR(ssm.getMetadata(1).tag2, 1.0, 1e-5);
109 }
110