joint_states_deflater_unittest.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
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 #include <gtest/gtest.h>
36 #include "joint_states_settler/joint_states_deflater.h"
37 
38 using namespace std;
39 using namespace joint_states_settler;
40 using namespace sensor_msgs;
41 
42 static const double eps = 1e-10;
43 
44 TEST(JointStatesDeflator, easy1)
45 {
46  JointStatesDeflater deflater;
47 
48  vector<string> joint_names;
49  joint_names.resize(2);
50  joint_names[0] = "A";
51  joint_names[1] = "C";
52  deflater.setDeflationJointNames(joint_names);
53 
54  JointStatePtr joint_states;
55  DeflatedJointStates deflated_msg;
56  JointState pruned;
57 
58  // ***********************************************
59 
60  joint_states.reset(new JointState);
61  joint_states->name.resize(3);
62  joint_states->position.resize(3);
63  joint_states->velocity.resize(3);
64  joint_states->effort.resize(3);
65  joint_states->name[0] = "A";
66  joint_states->name[1] = "B";
67  joint_states->name[2] = "C";
68  joint_states->position[0] = 1.0;
69  joint_states->position[1] = 2.0;
70  joint_states->position[2] = 3.0;
71 
72  deflater.deflate(joint_states, deflated_msg);
73  deflater.prune(*joint_states, pruned);
74 
75  ASSERT_EQ(deflated_msg.channels_.size(), (unsigned int) 2);
76  EXPECT_NEAR(deflated_msg.channels_[0], 1.0, eps);
77  EXPECT_NEAR(deflated_msg.channels_[1], 3.0, eps);
78 
79  ASSERT_EQ(pruned.name.size(), (unsigned int) 2);
80  ASSERT_EQ(pruned.position.size(), (unsigned int) 2);
81  EXPECT_EQ(pruned.name[0], "A");
82  EXPECT_EQ(pruned.name[1], "C");
83  EXPECT_NEAR(pruned.position[0], 1.0, eps);
84  EXPECT_NEAR(pruned.position[1], 3.0, eps);
85 
86  // ***********************************************
87 
88  joint_states.reset(new JointState);
89  joint_states->name.resize(3);
90  joint_states->position.resize(3);
91  joint_states->velocity.resize(3);
92  joint_states->effort.resize(3);
93  joint_states->name[0] = "C";
94  joint_states->name[1] = "A";
95  joint_states->name[2] = "B";
96  joint_states->position[0] = 4.0;
97  joint_states->position[1] = 5.0;
98  joint_states->position[2] = 6.0;
99 
100  deflater.deflate(joint_states, deflated_msg);
101  deflater.prune(*joint_states, pruned);
102 
103  EXPECT_EQ(deflated_msg.channels_.size(), (unsigned int) 2);
104  EXPECT_NEAR(deflated_msg.channels_[0], 5.0, eps);
105  EXPECT_NEAR(deflated_msg.channels_[1], 4.0, eps);
106 
107  ASSERT_EQ(pruned.name.size(), (unsigned int) 2);
108  ASSERT_EQ(pruned.position.size(), (unsigned int) 2);
109  EXPECT_EQ(pruned.name[0], "A");
110  EXPECT_EQ(pruned.name[1], "C");
111  EXPECT_NEAR(pruned.position[0], 5.0, eps);
112  EXPECT_NEAR(pruned.position[1], 4.0, eps);
113 
114  // ***********************************************
115 
116  joint_states.reset(new JointState);
117  joint_states->name.resize(4);
118  joint_states->position.resize(4);
119  joint_states->velocity.resize(4);
120  joint_states->effort.resize(4);
121  joint_states->name[0] = "D";
122  joint_states->name[1] = "C";
123  joint_states->name[2] = "B";
124  joint_states->name[3] = "A";
125  joint_states->position[0] = 7.0;
126  joint_states->position[1] = 8.0;
127  joint_states->position[2] = 9.0;
128  joint_states->position[3] = 10.0;
129 
130  deflater.deflate(joint_states, deflated_msg);
131  deflater.prune(*joint_states, pruned);
132 
133  EXPECT_EQ(deflated_msg.channels_.size(), (unsigned int) 2);
134  EXPECT_NEAR(deflated_msg.channels_[0], 10.0, eps);
135  EXPECT_NEAR(deflated_msg.channels_[1], 8.0, eps);
136 
137  ASSERT_EQ(pruned.name.size(), (unsigned int) 2);
138  ASSERT_EQ(pruned.position.size(), (unsigned int) 2);
139  EXPECT_EQ(pruned.name[0], "A");
140  EXPECT_EQ(pruned.name[1], "C");
141  EXPECT_NEAR(pruned.position[0], 10.0, eps);
142  EXPECT_NEAR(pruned.position[1], 8.0, eps);
143 }
144 
145 
146 int main(int argc, char **argv){
147  testing::InitGoogleTest(&argc, argv);
148  return RUN_ALL_TESTS();
149 }
joint_states_settler::JointStatesDeflater::prune
void prune(const sensor_msgs::JointState &joint_states, sensor_msgs::JointState &pruned_joint_states)
Remove all the joints that we don&#39;t care about.
Definition: joint_states_deflater.cpp:80
joint_states_settler
Definition: deflated_joint_states.h:41
TEST
TEST(JointStatesDeflator, easy1)
Definition: joint_states_deflater_unittest.cpp:44
main
int main(int argc, char **argv)
Definition: joint_states_deflater_unittest.cpp:146
sensor_msgs
settlerlib::Deflated::channels_
std::vector< double > channels_
joint_states_settler::DeflatedJointStates
Definition: deflated_joint_states.h:44
joint_states_settler::JointStatesDeflater::deflate
void deflate(const sensor_msgs::JointStateConstPtr &joint_states, DeflatedJointStates &deflated_elem)
Perform the deflation on a joint_states message.
Definition: joint_states_deflater.cpp:52
joint_states_deflater.h
joint_states_settler::JointStatesDeflater::setDeflationJointNames
void setDeflationJointNames(std::vector< std::string > joint_names)
Specify which joints to extract.
Definition: joint_states_deflater.cpp:46
joint_states_settler::JointStatesDeflater
Given a set a joint names, efficiently extracts a subset of joint positions.
Definition: joint_states_deflater.h:52
eps
static const double eps
Definition: joint_states_deflater_unittest.cpp:42

joint_states_settler
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Fri Apr 2 2021 02:13:00