MultiIdSceneTest.cpp
Go to the documentation of this file.
1 
20 #define BOOST_TEST_STATIC_LINK
21 
22 #include <boost/test/included/unit_test.hpp>
23 #include "next_best_view/test_cases/BaseTest.h"
24 #include "asr_next_best_view/GetAttributedPointCloud.h"
25 
26 using namespace next_best_view;
27 using namespace boost::unit_test;
28 
29 class MultiIdSceneTest : public BaseTest{
30 public:
31  MultiIdSceneTest() : BaseTest (){
32  }
33 
34  virtual ~MultiIdSceneTest() {}
35 
36  void iterationTest() {
37  asr_next_best_view::GetAttributedPointCloud gpc;
38  asr_next_best_view::SetAttributedPointCloud apc;
39 
40  ROS_INFO("Generating point cloud");
41 
42  int hpSize_scene1 = 6;
43  int hpSize_scene2 = 3;
44  SimpleVector3* hp_scene1 = new SimpleVector3[hpSize_scene1];
45  SimpleVector3* hp_scene2 = new SimpleVector3[hpSize_scene2];
46  hp_scene1[0] = SimpleVector3(0.725892364979, 1.57344818115, 0.8);
47  hp_scene1[1] = SimpleVector3(1.0500099659, 1.63358879089, 0.8);
48  hp_scene1[2] = SimpleVector3(1.66446590424, -0.549417376518, 0.8);
49  hp_scene1[3] = SimpleVector3(1.92009782791,-0.298665702343, 0.8);
50  hp_scene1[4] = SimpleVector3(-0.978491704464, 0.583356167078, 0.8);
51  hp_scene1[5] = SimpleVector3(-0.972244024277, 0.759688913822, 0.8);
52  hp_scene2[0] = SimpleVector3(-0.404876768589,-0.50940537452, 1.3);
53  hp_scene2[1] = SimpleVector3(-0.404876768589,-0.50940537452, 1.3);
54  hp_scene2[2] = SimpleVector3(-0.404876768589,-0.50940537452, 1.7);
55 
56  SimpleQuaternion* orientation_scene1 = new SimpleQuaternion[hpSize_scene1];
57  SimpleQuaternion* orientation_scene2 = new SimpleQuaternion[hpSize_scene2];
58  orientation_scene1[0] = euler2Quaternion(-90, 0.0, 0.0);
59  orientation_scene1[1] = euler2Quaternion(-90, 0.0, 0.0);
60  orientation_scene1[2] = euler2Quaternion(-90, 100.0, 0.0);
61  orientation_scene1[3] = euler2Quaternion(-90, 170.0, 0.0);
62  orientation_scene1[4] = euler2Quaternion(-90, -90.0, 0.0);
63  orientation_scene1[5] = euler2Quaternion(-90, -90.0, 0.0);
64  orientation_scene2[0] = euler2Quaternion(-90, 180.0, 0.0);
65  orientation_scene2[1] = euler2Quaternion(-90, -90.0, 0.0);
66  orientation_scene2[2] = euler2Quaternion(-90, 180, 0.0);
67 
68  std::string* types_scene1 = new std::string[hpSize_scene1];
69  std::string* types_scene2 = new std::string[hpSize_scene2];
70  types_scene1[0] = "Cup";
71  types_scene1[1] = "PlateDeep";
72  types_scene1[2] = "CoffeeBox";
73  types_scene1[3] = "CoffeeFilters2";
74  types_scene1[4] = "Smacks";
75  types_scene1[5] = "VitalisSchoko";
76  types_scene2[0] = "CeylonTea";
77  types_scene2[1] = "CeylonTea";
78  types_scene2[2] = "CeylonTea";
79 
80 
81  int sampleSize = 10;
82  std::map<std::string, std::vector<asr_msgs::AsrAttributedPoint>* > objectPointCloudsMap;
83 
84  for (std::size_t idx = 0; idx < (std::size_t)hpSize_scene1; idx++) {
85 
86  if(objectPointCloudsMap.find(types_scene1[idx]) == objectPointCloudsMap.end())
87  {
88  objectPointCloudsMap[types_scene1[idx]]= new std::vector<asr_msgs::AsrAttributedPoint>();
89  }
90  for (std::size_t cnt = 0; cnt < (std::size_t)sampleSize; cnt++) {
91  SimpleVector3 randomVector;
92  randomVector = MathHelper::getRandomVector(hp_scene1[idx], SimpleVector3(.05, .05, 0.01));
93 
94  asr_msgs::AsrAttributedPoint element;
95 
96  geometry_msgs::Pose pose;
97  pose.orientation.w = orientation_scene1[idx].w();
98  pose.orientation.x = orientation_scene1[idx].x();
99  pose.orientation.y = orientation_scene1[idx].y();
100  pose.orientation.z = orientation_scene1[idx].z();
101  pose.position.x = randomVector[0];
102  pose.position.y = randomVector[1];
103  pose.position.z = randomVector[2];
104 
105  element.type = types_scene1[idx];
106  element.identifier = std::to_string(idx);
107  element.pose = pose;
108  objectPointCloudsMap[types_scene1[idx]]->push_back(element);
109  apc.request.point_cloud.elements.push_back(element);
110  }
111  }
112 
113  ROS_INFO("Setting initial pose");
114  geometry_msgs::Pose initialPose;
115  initialPose.position.x = 1.04865884781;
116  initialPose.position.y = 0.846048951149;
117  initialPose.position.z = 1.32;
118 
119  initialPose.orientation.w = 0.668036938496;
120  initialPose.orientation.x = 0.0;
121  initialPose.orientation.y = 0.0;
122  initialPose.orientation.z = 0.744128113166;
123  this->setInitialPose(initialPose);
124 
125  // set point cloud
126  if (!mSetPointCloudClient.call(apc.request, apc.response)) {
127  ROS_ERROR("Could not set initial point cloud.");
128  }
129 
130  if (!mGetPointCloudClient.call(gpc)) {
131  ROS_ERROR("Could not get initial point cloud.");
132  }
133 
134  ros::Rate r(2);
135  asr_next_best_view::GetNextBestView nbv;
136  nbv.request.current_pose = initialPose;
137 
138  int x = 1;
139  bool scene2_isInitialized = false;
140  bool setPointCloud = false;
141  while(ros::ok()) {
142 
143  if(apc.request.point_cloud.elements.size() == 0)
144  {
145  ROS_INFO("No elements were found");
146  break;
147  }
148  else if(setPointCloud)
149  {
150  setPointCloud = false;
151  if (!mSetPointCloudClient.call(apc.request, apc.response)) {
152  ROS_ERROR("Could not set point cloud.");
153  break;
154  }
155  }
156 
157  ROS_INFO_STREAM("Calculating NBV #" << x);
158  if (!mGetNextBestViewClient.call(nbv.request, nbv.response)) {
159  ROS_ERROR("Something went wrong in next best view");
160  break;
161  }
162  if (nbv.response.viewport.object_type_name_list.size() > 0)
163  {
164  if (!mGetPointCloudClient.call(gpc)) {
165  ROS_ERROR("Could not get point cloud.");
166  break;
167  }
168  apc.request.point_cloud.elements.clear();
169  apc.request.point_cloud.elements.insert(apc.request.point_cloud.elements.end(), gpc.response.point_cloud.elements.begin(), gpc.response.point_cloud.elements.end());
170 
171  for(unsigned int i=0;i<nbv.response.viewport.object_type_name_list.size();i++)
172  {
173  if (nbv.response.viewport.object_type_name_list[i] != "CeylonTea")
174  {
175  std::vector<asr_msgs::AsrAttributedPoint> temp;
176  for (std::vector<asr_msgs::AsrAttributedPoint>::iterator it = apc.request.point_cloud.elements.begin(); it != apc.request.point_cloud.elements.end(); ++it)
177  {
178  if ((nbv.response.viewport.object_type_name_list[i].compare(it->type)) != 0)
179  {
180  temp.push_back(*it);
181  }
182  }
183  apc.request.point_cloud.elements.clear();
184  apc.request.point_cloud.elements.insert(apc.request.point_cloud.elements.end(), temp.begin(), temp.end());
185  setPointCloud = true;
186  }
187  if (nbv.response.viewport.object_type_name_list[i] == "Smacks" && !scene2_isInitialized)
188  {
189  for (std::size_t idx = 0; idx < (std::size_t)hpSize_scene2; idx++) {
190 
191  if(objectPointCloudsMap.find(types_scene2[idx]) == objectPointCloudsMap.end())
192  {
193  objectPointCloudsMap[types_scene2[idx]]= new std::vector<asr_msgs::AsrAttributedPoint>();
194  }
195  for (std::size_t cnt = 0; cnt < (std::size_t)sampleSize; cnt++) {
196  SimpleVector3 randomVector;
197  randomVector = MathHelper::getRandomVector(hp_scene2[idx], SimpleVector3(.05, .05, 0.01));
198  asr_msgs::AsrAttributedPoint element;
199  geometry_msgs::Pose pose;
200  pose.orientation.w = orientation_scene2[idx].w();
201  pose.orientation.x = orientation_scene2[idx].x();
202  pose.orientation.y = orientation_scene2[idx].y();
203  pose.orientation.z = orientation_scene2[idx].z();
204  pose.position.x = randomVector[0];
205  pose.position.y = randomVector[1];
206  pose.position.z = randomVector[2];
207  element.type = types_scene2[idx];
208  element.identifier = std::to_string(idx + hpSize_scene1);
209  element.pose = pose;
210  apc.request.point_cloud.elements.push_back(element);
211  }
212  }
213  scene2_isInitialized = true;
214  setPointCloud = true;
215  }
216  }
217  }
218 
219 
220  if (!nbv.response.found) {
221  ROS_ERROR("No NBV found");
222  break;
223  }
224 
225  asr_next_best_view::UpdatePointCloud upc_req;
226  upc_req.request.object_type_name_list = nbv.response.viewport.object_type_name_list;
227  upc_req.request.pose_for_update = nbv.response.viewport.pose;
228  if(!mUpdatePointCloudClient.call(upc_req)) {
229  ROS_ERROR("Update Point Cloud failed!");
230  break;
231  }
232  x++;
233 
234  nbv.request.current_pose = nbv.response.viewport.pose;
235 
236  ros::spinOnce();
237  waitForEnter();
238  ros::Duration(2).sleep();
239  }
240  }
241 };
242 
243 test_suite* init_unit_test_suite( int argc, char* argv[] ) {
244  ros::init(argc, argv, "nbv_test");
245  ros::start();
246 
247  test_suite* evaluation = BOOST_TEST_SUITE("Evaluation NBV");
248 
249  boost::shared_ptr<MultiIdSceneTest> testPtr(new MultiIdSceneTest());
250 
251  evaluation->add(BOOST_CLASS_TEST_CASE(&MultiIdSceneTest::iterationTest, testPtr));
252 
253  framework::master_test_suite().add(evaluation);
254 
255  return 0;
256 }
257 
258 
MultiIdSceneTest::~MultiIdSceneTest
virtual ~MultiIdSceneTest()
Definition: MultiIdSceneTest.cpp:34
ros::start
ROSCPP_DECL void start()
next_best_view::SimpleVector3
Eigen::Matrix< Precision, 3, 1 > SimpleVector3
Definition: typedef.hpp:53
ros::Duration::sleep
bool sleep() const
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::shared_ptr
unit_test
next_best_view
this namespace contains all generally usable classes.
Definition: CameraModelFilter.hpp:26
ROS_INFO
#define ROS_INFO(...)
ros::ok
ROSCPP_DECL bool ok()
next_best_view::MathHelper::getRandomVector
static SimpleVectorX getRandomVector(const SimpleVectorX &mean, const SimpleVectorX &standardDeviation)
Definition: MathHelper.cpp:104
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
init_unit_test_suite
test_suite * init_unit_test_suite(int argc, char *argv[])
Definition: MultiIdSceneTest.cpp:243
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
next_best_view::SimpleQuaternion
Eigen::Quaternion< Precision > SimpleQuaternion
Definition: typedef.hpp:67
ros::spinOnce
ROSCPP_DECL void spinOnce()
ROS_ERROR
#define ROS_ERROR(...)

asr_next_best_view
Author(s): Aumann Florian, Borella Jocelyn, Heller Florian, Meißner Pascal, Schleicher Ralf, Stöckle Patrick, Stroh Daniel, Trautmann Jeremias, Walter Milena, Wittenbeck Valerij
autogenerated on Thu Jan 9 2020 07:20:18