NextBestView.hpp
Go to the documentation of this file.
1 
20 #pragma once
21 
22 // Global Includes
23 #include <algorithm>
24 #include <boost/foreach.hpp>
25 #include <eigen3/Eigen/Dense>
26 #include <pcl-1.7/pcl/point_cloud.h>
27 #include <pcl_conversions/pcl_conversions.h>
28 #include <vector>
29 
30 // ROS Main Include
31 #include <ros/ros.h>
32 
33 // ROS Includes
34 #include <object_database/ObjectManager.h>
35 #include <dynamic_reconfigure/server.h>
36 #include <asr_next_best_view/DynamicParametersConfig.h>
37 
38 // Local Includes
39 #include "typedef.hpp"
40 #include "asr_next_best_view/RatedViewport.h"
41 #include "asr_next_best_view/RateViewports.h"
42 #include "asr_next_best_view/RemoveObjects.h"
43 #include "asr_next_best_view/NormalsInfo.h"
44 #include "asr_next_best_view/TriggerFrustumVisualization.h"
45 #include "asr_next_best_view/TriggerFrustumsAndPointCloudVisualization.h"
46 #include "asr_next_best_view/GetAttributedPointCloud.h"
47 #include "asr_next_best_view/GetNextBestView.h"
48 #include "asr_next_best_view/SetAttributedPointCloud.h"
49 #include "asr_next_best_view/SetInitRobotState.h"
50 #include "asr_next_best_view/ResetCalculator.h"
51 #include "asr_next_best_view/UpdatePointCloud.h"
55 #include "asr_msgs/AsrAttributedPointCloud.h"
56 #include "asr_msgs/AsrAttributedPoint.h"
57 #include "asr_msgs/AsrViewport.h"
68 #include <robot_model_services/robot_model/impl/MILDRobotModelWithExactIK.hpp>
69 #include <robot_model_services/robot_model/impl/MILDRobotModelWithApproximatedIK.hpp>
70 #include <robot_model_services/robot_model/impl/MILDRobotModelWithExactIKFactory.hpp>
71 #include <robot_model_services/robot_model/impl/MILDRobotModelWithApproximatedIKFactory.hpp>
72 #include <robot_model_services/robot_model/impl/MILDRobotState.hpp>
85 
86 namespace next_best_view {
87 // Defining namespace shorthandles
88 namespace viz = visualization_msgs;
89 namespace odb = object_database;
90 
97 class NextBestView {
98 private:
99  // The Calculator Instance
101 
102  // Node Handles
105 
106  // ServiceServer and Publisher
118 
119 
120  // Etcetera
129 
130  // dynconfig
131  dynamic_reconfigure::Server<asr_next_best_view::DynamicParametersConfig> mDynamicReconfigServer;
132  asr_next_best_view::DynamicParametersConfig mConfig;
133  uint32_t mConfigLevel;
134  bool mFirstRun;
136  parameterConfig = 0b1 << 0,
138  mapHelperConfig = 0b1 << 2,
139  spaceSamplingConfig = (0b1 << 3) | mapHelperConfig, // these 3 might require mapHelper (depending on chosen moduleId),
140  cameraModelConfig = (0b1 << 4) | mapHelperConfig, // so they should be updated if mapHelper params are changed
142  ratingConfig = (0b1 << 6) | robotModelConfig | cameraModelConfig, // rating requires cameraModel and robotModel
143  hypothesisUpdaterConfig = (0b1 << 7) | ratingConfig, // requires ratingModule
144  cropboxFileConfig = 0b1 << 8,
146  };
147 
149 
150  // module factory classes
155  robot_model_services::RobotModelAbstractFactoryPtr mRobotModelFactoryPtr;
158 
159 public:
164  {
165  mDebugHelperPtr = DebugHelper::getInstance();
166 
167  mGlobalNodeHandle = ros::NodeHandle();
168  mNodeHandle = ros::NodeHandle(ros::this_node::getName());
169 
170  mFirstRun = true;
171  dynamic_reconfigure::Server<asr_next_best_view::DynamicParametersConfig>::CallbackType f = boost::bind(&NextBestView::dynamicReconfigureCallback, this, _1, _2);
172  mDynamicReconfigServer.setCallback(f);
173 
174  // TODO is this necessary or does dynReconfig always call callback once at the beginning?
175  // DynamicParametersConfig config;
176  // mDynamicReconfigServer.getConfigDefault(config);
177  // dynamicReconfigureCallback(config, UINT_MAX);
178  }
179 
180  //dtor
181  virtual ~NextBestView() {
182  this->mVisualizationThread->join();
183  }
184 
185  void initialize()
186  {
187  mDebugHelperPtr->writeNoticeably("STARTING NBV PARAMETER OUTPUT", DebugHelper::PARAMETERS);
188 
189  mDebugHelperPtr->write(std::stringstream() << "debugLevels: " << mDebugHelperPtr->getLevelString(), DebugHelper::PARAMETERS);
190 
191  mCurrentlyPublishingVisualization = false;
192 
193  /* These are the parameters for the CameraModelFilter. By now they will be kept in here, but for the future they'd better
194  * be defined in the CameraModelFilter specialization class.
195  */
196  mDebugHelperPtr->write(std::stringstream() << "fovx: " << mConfig.fovx, DebugHelper::PARAMETERS);
197  mDebugHelperPtr->write(std::stringstream() << "fovy: " << mConfig.fovy, DebugHelper::PARAMETERS);
198  mDebugHelperPtr->write(std::stringstream() << "ncp: " << mConfig.ncp, DebugHelper::PARAMETERS);
199  mDebugHelperPtr->write(std::stringstream() << "fcp: " << mConfig.fcp, DebugHelper::PARAMETERS);
200  mDebugHelperPtr->write(std::stringstream() << "radius: " << mConfig.radius, DebugHelper::PARAMETERS);
201  mDebugHelperPtr->write(std::stringstream() << "colThresh: " << mConfig.colThresh, DebugHelper::PARAMETERS);
202  mDebugHelperPtr->write(std::stringstream() << "samples: " << mConfig.sampleSizeUnitSphereSampler, DebugHelper::PARAMETERS);
203  mDebugHelperPtr->write(std::stringstream() << "speedFactorRecognizer: " << mConfig.speedFactorRecognizer, DebugHelper::PARAMETERS);
204 
206  // HERE STARTS THE CONFIGURATION OF THE NEXTBESTVIEW CALCULATOR //
208  /* The NextBestViewCalculator consists of multiple modules which interact with each other. By this
209  * interaction we get a result for the next best view which suites our constraints best.
210  *
211  * The modules namely are:
212  * - UnitSphereSampler (unit_sphere_sampler/UnitSphereSampler.hpp)
213  * - SpaceSampler (space_sampler/SpaceSampler.hpp)
214  * - CameraModelFilter (camera_model_filter/CameraModelFilter.hpp)
215  * - RobotModel (robot_model/RobotModel.hpp)
216  * - RatingModule (rating_module/RatingModule.hpp)
217  * - HypothesisUpdater (hypothesis_updater/HypothesisUpdater.hpp)
218  *
219  * Every of these modules is an abstract class which provides an interface to interact with.
220  * These interfaces are, right now, far from final and can therefore be change as you want to change them.
221  * Side-effects are expected just in the next_best_view node.
222  */
223 
224  /* SpiralApproxUnitSphereSampler is a specialization of the abstract UnitSphereSampler class.
225  * It picks a given number of samples out of the unit sphere. These samples should be uniform
226  * distributed on the sphere's surface which is - by the way - no easy problem to solve.
227  * Therefore we used an approximation algorithm which approximates the surface with a
228  * projection of a spiral on the sphere's surface. Resulting in this wonderful sounding name.
229  * - sphereSamplerId == 1 => SpiralApproxUnitSphereSampler
230  */
231  if (mConfigLevel & sphereSamplingConfig) {
232  if (mSphereSamplerFactoryPtr) {
233  mSphereSamplerFactoryPtr.reset();
234  }
235  mSphereSamplerFactoryPtr = createSphereSamplerFromConfig(mConfig.sphereSamplerId);
236  mCalculator.setUnitSphereSampler(mSphereSamplerFactoryPtr->createUnitSphereSampler());
237  }
238 
239  /* MapHelper does get the maps on which we are working on and modifies them for use with applications like raytracing and others.
240  */
241  if (mConfigLevel & mapHelperConfig) {
242  mMapHelperPtr = MapHelperPtr(new MapHelper());
243  mMapHelperPtr->setCollisionThreshold(mConfig.colThresh);
244  mCalculator.setMapHelper(mMapHelperPtr);
245 
246  mVisHelperPtr = VisualizationHelperPtr(new VisualizationHelper(mCalculator.getMapHelper()));
247  }
248 
249 
250  /* Intializes spaceSampler with a SpaceSampler subclass specified by the parameter samplerId :
251  * - samplerId == 1 => MapBasedHexagonSpaceSampler
252  * - samplerId == 2 => MapBasedRandomSpaceSampler
253  * - samplerId == 3 => PlaneSubSpaceSampler
254  * - samplerId == 4 => Raytracing2DBasedSpaceSampler
255  */
256  mDebugHelperPtr->write(std::stringstream() << "sampleSizeMapBasedRandomSpaceSampler: " << mConfig.sampleSizeMapBasedRandomSpaceSampler, DebugHelper::PARAMETERS);
257  mDebugHelperPtr->write(std::stringstream() << "spaceSamplerId: " << mConfig.spaceSamplerId, DebugHelper::PARAMETERS);
258 
259  if (mConfigLevel & spaceSamplingConfig) {
260  if (mSpaceSampleFactoryPtr) {
261  mSpaceSampleFactoryPtr.reset();
262  }
263  mSpaceSampleFactoryPtr = createSpaceSamplerFromConfig(mConfig.spaceSamplerId);
264  mCalculator.setSpaceSampler(mSpaceSampleFactoryPtr->createSpaceSampler());
265  }
266 
267  mDebugHelperPtr->write(std::stringstream() << "cameraFilterId: " << mConfig.cameraFilterId, DebugHelper::PARAMETERS);
268 
269  /*
270  * Intializes cameraModelFilter with a CameraModelFilter subclass specified by the parameter cameraFilterId :
271  * - cameraFilterId == 1 => StereoCameraModelFilter
272  * - cameraFilterId == 2 => SingleCameraModelFilter
273  */
274  if (mConfigLevel & cameraModelConfig) {
275  if (mCameraModelFactoryPtr) {
276  mCameraModelFactoryPtr.reset();
277  }
278  mCameraModelFactoryPtr = createCameraModelFromConfig(mConfig.cameraFilterId);
279  mCalculator.setCameraModelFilter(mCameraModelFactoryPtr->createCameraModelFilter());
280  mCalculator.setCameraModelFilterAbstractFactoryPtr(mCameraModelFactoryPtr);
281  }
282 
283  /* MILDRobotModel is a specialization of the abstract RobotModel class.
284  * The robot model maps takes the limitations of the used robot into account and by this it is possible to filter out
285  * non-reachable configurations of the robot which can therefore be ignored during calculation.
286  * - robotModelId == 1 => MILDRobotModelWithExactInverseKinematic
287  * - robotModelId == 2 => MILDRobotModelWithApproximatedIinverseKinematic
288  */
289  mDebugHelperPtr->write(std::stringstream() << "panMin: " << mConfig.panMin, DebugHelper::PARAMETERS);
290  mDebugHelperPtr->write(std::stringstream() << "panMax: " << mConfig.panMax, DebugHelper::PARAMETERS);
291  mDebugHelperPtr->write(std::stringstream() << "tiltMin: " << mConfig.tiltMin, DebugHelper::PARAMETERS);
292  mDebugHelperPtr->write(std::stringstream() << "tiltMax: " << mConfig.tiltMax, DebugHelper::PARAMETERS);
293 
294  if (mConfigLevel & robotModelConfig) {
295  if (mRobotModelFactoryPtr) {
296  mRobotModelFactoryPtr.reset();
297  }
298  mRobotModelFactoryPtr = createRobotModelFromConfig(mConfig.robotModelId);
299  mCalculator.setRobotModel(mRobotModelFactoryPtr->createRobotModel());
300  }
301 
302  /* DefaultRatingModule is a specialization of the abstract RatingModule class.
303  * The rating module calculates the use and the costs of an operation.
304  * - ratingModuleId == 1 => DefaultRatingModule
305  */
306  mDebugHelperPtr->write(std::stringstream() << "useTargetRobotState: " << mConfig.useTargetRobotState, DebugHelper::PARAMETERS);
307 
308  if (mConfigLevel & ratingConfig) {
309  if (mRatingModuleFactoryPtr) {
310  mRatingModuleFactoryPtr.reset();
311  }
312  mRatingModuleFactoryPtr = createRatingModuleFromConfig(mConfig.ratingModuleId);
313  mCalculator.setRatingModule(mRatingModuleFactoryPtr->createRatingModule());
314  mCalculator.setRatingModuleAbstractFactoryPtr(mRatingModuleFactoryPtr);
315  }
316 
317  /* PerspectiveHypothesisUpdater is a specialization of the abstract HypothesisUpdater.
318  * - hypothesisUpdaterId == 1 => PerspectiveHypothesisUpdater
319  * - hypothesisUpdaterId == 2 => DefaultHypothesisUpdater
320  */
321  if (mConfigLevel & hypothesisUpdaterConfig) {
322  if (mHypothesisUpdaterFactoryPtr) {
323  mHypothesisUpdaterFactoryPtr.reset();
324  }
325  mHypothesisUpdaterFactoryPtr = createHypothesisUpdaterFromConfig(mConfig.hypothesisUpdaterId);
326  mCalculator.setHypothesisUpdater(mHypothesisUpdaterFactoryPtr->createHypothesisUpdater());
327  }
328 
329  // The settings of the modules.
330  if (mConfigLevel & cropboxFileConfig) {
331  mCalculator.loadCropBoxListFromFile(mConfig.mCropBoxListFilePath);
332  }
333  if (mConfigLevel & parameterConfig) {
334  mDebugHelperPtr->setLevels(mConfig.debugLevels);
335  mCalculator.setNumberOfThreads(mConfig.nRatingThreads);
336  mCalculator.setEnableCropBoxFiltering(mConfig.enableCropBoxFiltering);
337  mCalculator.setEnableIntermediateObjectWeighting(mConfig.enableIntermediateObjectWeighting);
338  //Set the max amout of iterations
339  mDebugHelperPtr->write(std::stringstream() << "maxIterationSteps: " << mConfig.maxIterationSteps, DebugHelper::PARAMETERS);
340  mCalculator.setMaxIterationSteps(mConfig.maxIterationSteps);
341  mCalculator.setEpsilon(mConfig.epsilon);
342  float minUtility;
343  mNodeHandle.getParam("/scene_exploration_sm/min_utility_for_moving", minUtility);
344  mCalculator.setMinUtility(minUtility);
345 
346  mShowPointcloud = mConfig.show_point_cloud;
347  mShowFrustumPointCloud = mConfig.show_frustum_point_cloud;
348  mShowFrustumMarkerArray = mConfig.show_frustum_marker_array;
349  mShowNormals = mConfig.show_normals;
350  }
351 
352  mDebugHelperPtr->write(std::stringstream() << "boolClearBetweenIterations: " << mVisHelperPtr->getBoolClearBetweenIterations(), DebugHelper::PARAMETERS);
353 
354  mDebugHelperPtr->writeNoticeably("ENDING NBV PARAMETER OUTPUT", DebugHelper::PARAMETERS);
355 
356  if (mFirstRun) {
357  // at our first run we have to advertise services.
358  mFirstRun = false;
359  mGetPointCloudServiceServer = mNodeHandle.advertiseService("get_point_cloud", &NextBestView::processGetPointCloudServiceCall, this);
360  mGetNextBestViewServiceServer = mNodeHandle.advertiseService("next_best_view", &NextBestView::processGetNextBestViewServiceCall, this);
361  mSetPointCloudServiceServer = mNodeHandle.advertiseService("set_point_cloud", &NextBestView::processSetPointCloudServiceCall, this);
362  mSetInitRobotStateServiceServer = mNodeHandle.advertiseService("set_init_robot_state", &NextBestView::processSetInitRobotStateServiceCall, this);
363  mUpdatePointCloudServiceServer = mNodeHandle.advertiseService("update_point_cloud", &NextBestView::processUpdatePointCloudServiceCall, this);
364  mTriggerFrustumVisualizationServer = mNodeHandle.advertiseService("trigger_frustum_visualization", &NextBestView::processTriggerFrustumVisualization, this);
365  mTriggerOldFrustumVisualizationServer = mNodeHandle.advertiseService("trigger_old_frustum_visualization", &NextBestView::processTriggerOldFrustumVisualization, this);
366  mTriggerFrustmsAndPointCloudVisualizationServer = mNodeHandle.advertiseService("trigger_frustums_and_point_cloud_visualization", &NextBestView::processTriggerFrustumsAndPointCloudVisualization, this);
367  mResetCalculatorServer = mNodeHandle.advertiseService("reset_nbv_calculator", &NextBestView::processResetCalculatorServiceCall, this);
368  mRateViewportsServer = mNodeHandle.advertiseService("rate_viewports", &NextBestView::processRateViewports, this);
369  mRemoveObjectsServer = mNodeHandle.advertiseService("remove_objects", &NextBestView::processRemoveObjects, this);
370  }
371  }
372 
373  /* The UnitSphereSampler generates orientations to generate views.
374  *
375  */
377  switch (moduleId) {
378  case 1:
379  return UnitSphereSamplerAbstractFactoryPtr(new SpiralApproxUnitSphereSamplerFactory(mConfig.sampleSizeUnitSphereSampler));
380  default:
381  std::stringstream ss;
382  ss << mConfig.sphereSamplerId << " is not a valid sphere sampler ID";
383  ROS_ERROR_STREAM(ss.str());
384  throw std::runtime_error(ss.str());
385  }
386  }
387 
388  /* SpaceSampler generates positions to generate views.
389  * The diffrent sampler samples in diffrent ways the given xy-plane where the robot can stay for the next view.
390  */
392  switch (moduleId)
393  {
394  case 1:
395  /* MapBasedHexagonSpaceSampler is a specialization of the abstract SpaceSampler class.
396  * By space we denote the area in which the robot is moving. In our case there are just two degrees of freedom
397  * in which the robot can move, namely the xy-plane. But we do also have a map on which we can base our sampling on.
398  * There are a lot of ways to sample the xy-plane into points but we decided to use a hexagonal grid which we lay over
399  * the map and calculate the points which are contained in the feasible map space.
400  */
401  return SpaceSamplerAbstractFactoryPtr(new MapBasedHexagonSpaceSamplerFactory(mMapHelperPtr, mConfig.radius));
402  case 2:
403  return SpaceSamplerAbstractFactoryPtr(new MapBasedRandomSpaceSamplerFactory(mMapHelperPtr, mConfig.sampleSizeMapBasedRandomSpaceSampler));
404  case 3:
406  case 4:
408  default:
409  std::stringstream ss;
410  ss << mConfig.spaceSamplerId << " is not a valid space sampler ID";
411  ROS_ERROR_STREAM(ss.str());
412  throw std::runtime_error(ss.str());
413  }
414  }
415 
416  /* CameraModelFilter used to find hypotheses inside a camera frustum
417  * Case 1 use the fustrum of both cameras of the stereo camera. Hypotheses are placed sectional area of the fustrum.
418  * Case 2 hypotheses ware only placed in one fustrum of one camera.
419  */
421  SimpleVector3 leftCameraPivotPointOffset = SimpleVector3(0.0, -0.067, 0.04);
422  SimpleVector3 rightCameraPivotPointOffset = SimpleVector3(0.0, 0.086, 0.04);
423  SimpleVector3 oneCameraPivotPointOffset = (leftCameraPivotPointOffset + rightCameraPivotPointOffset) * 0.5;
424 
425  switch (moduleId) {
426  case 1:
428  new StereoCameraModelFilterFactory(leftCameraPivotPointOffset, rightCameraPivotPointOffset,
429  mConfig.fovx, mConfig.fovy,
430  mConfig.fcp, mConfig.ncp,
431  mConfig.speedFactorRecognizer));
432  case 2:
434  new SingleCameraModelFilterFactory(oneCameraPivotPointOffset,
435  mConfig.fovx, mConfig.fovy,
436  mConfig.fcp, mConfig.ncp,
437  mConfig.speedFactorRecognizer));
438  default:
439  std::stringstream ss;
440  ss << mConfig.cameraFilterId << " is not a valid camera filter ID";
441  ROS_ERROR_STREAM(ss.str());
442  throw std::runtime_error(ss.str());
443  }
444  }
445 
446  /* Creates the RobotModel, with are used for kinematic calculations of the robot.
447  *
448  */
449 
450  robot_model_services::RobotModelAbstractFactoryPtr createRobotModelFromConfig(int moduleId) {
451  switch (moduleId) {
452  case 1:
453  mDebugHelperPtr->write("NBV: Using new IK model", DebugHelper::PARAMETERS);
454  return robot_model_services::RobotModelAbstractFactoryPtr(new robot_model_services::MILDRobotModelWithExactIKFactory(mConfig.tiltMin, mConfig.tiltMax, mConfig.panMin, mConfig.panMax));
455  case 2:
456  mDebugHelperPtr->write("NBV: Using old IK model", DebugHelper::PARAMETERS);
457  return robot_model_services::RobotModelAbstractFactoryPtr(new robot_model_services::MILDRobotModelWithApproximatedIKFactory(mConfig.tiltMin, mConfig.tiltMax, mConfig.panMin, mConfig.panMax));
458  default:
459  std::stringstream ss;
460  ss << mConfig.robotModelId << " is not a valid robot model ID";
461  ROS_ERROR_STREAM(ss.str());
462  throw std::runtime_error(ss.str());
463  }
464  }
465 
466  /* RatingModules are used to rate the gerenrated views.
467  */
469  robot_model_services::RobotModelAbstractFactoryPtr robotModelFactoryPtr;
470  CameraModelFilterAbstractFactoryPtr cameraModelFactoryPtr;
471  switch (moduleId) {
472  case 1:
473  robotModelFactoryPtr = createRobotModelFromConfig(mConfig.robotModelId);
474  cameraModelFactoryPtr = createCameraModelFromConfig(mConfig.cameraFilterId);
476  new DefaultRatingModuleFactory(mConfig.fovx, mConfig.fovy,
477  mConfig.fcp, mConfig.ncp,
478  mConfig.useTargetRobotState,
479  robotModelFactoryPtr, cameraModelFactoryPtr, mMapHelperPtr,
480  mConfig.mRatingNormalAngleThreshold / 180.0 * M_PI,
481  mConfig.mOmegaUtility, mConfig.mOmegaPan, mConfig.mOmegaTilt,
482  mConfig.mOmegaRot, mConfig.mOmegaBase, mConfig.mOmegaRecognizer,
483  mConfig.useOrientationUtility, mConfig.useProximityUtility, mConfig.useSideUtility));
484  default:
485  std::stringstream ss;
486  ss << mConfig.ratingModuleId << " is not a valid rating module ID";
487  ROS_ERROR_STREAM(ss.str());
488  throw std::runtime_error(ss.str());
489  }
490  }
491 
492  /*The HypothesisUpdater removes normales inside the frustum which are seen from the robot position (Camera View).
493  */
495  DefaultRatingModuleFactoryPtr ratingModuleFactoryPtr;
496  switch (moduleId) {
497  case 1:
498  // create a DefaultRatingModule from config
499  ratingModuleFactoryPtr = boost::static_pointer_cast<DefaultRatingModuleFactory>(createRatingModuleFromConfig(1));
501  mConfig.mHypothesisUpdaterAngleThreshold / 180.0 * M_PI));
502  case 2:
504  default:
505  std::stringstream ss;
506  ss << mConfig.hypothesisUpdaterId << " is not a valid hypothesis module ID";
507  ROS_ERROR_STREAM(ss.str());
508  throw std::runtime_error(ss.str());
509  }
510  }
511 
512  bool processResetCalculatorServiceCall(asr_next_best_view::ResetCalculator::Request &request, asr_next_best_view::ResetCalculator::Response &response) {
513  mDebugHelperPtr->writeNoticeably("STARTING NBV RESET-CALCULATOR SERVICE CALL", DebugHelper::SERVICE_CALLS);
514  mConfigLevel = std::numeric_limits<uint32_t>::max();
515  initialize();
516 
517  response.succeeded = true;
518  mDebugHelperPtr->writeNoticeably("ENDING NBV RESET-CALCULATOR SERVICE CALL", DebugHelper::SERVICE_CALLS);
519  return true;
520  }
521 
522  bool processRateViewports(asr_next_best_view::RateViewports::Request &request, asr_next_best_view::RateViewports::Response &response) {
523  mDebugHelperPtr->writeNoticeably("STARTING NBV RATE-VIEWPORTS SERVICE CALL", DebugHelper::SERVICE_CALLS);
524 
525  if (request.viewports.empty()) {
526  return true;
527  }
528 
529  // Current camera view (frame of camera) of the robot.
530  ViewportPoint currentCameraViewport(request.current_pose);
531 
532  // set robotstate
533  mCalculator.initializeRobotState(currentCameraViewport);
534 
535  // convert geometry_msgs::Poses to ViewportPoints
537  unsigned int i = 0;
538  for (geometry_msgs::Pose viewport : request.viewports) {
539  ViewportPoint sampleViewport(viewport);
540  sampleViewport.object_type_set = ObjectTypeSetPtr(new ObjectTypeSet(request.object_type_name_list.begin(), request.object_type_name_list.end()));
541  sampleViewport.oldIdx = i;
542  sampleViewportsPtr->push_back(sampleViewport);
543  i++;
544  }
545 
546  // find nearby object hypothesis per sampleViewport
547  IndicesPtr feasibleViewportsPtr;
548  mCalculator.getFeasibleViewports(sampleViewportsPtr, feasibleViewportsPtr);
549 
550  // remove sampleViewports without nearby object hypothesis
551  ViewportPointCloudPtr feasibleSampleViewportsPtr = ViewportPointCloudPtr(new ViewportPointCloud(*sampleViewportsPtr, *feasibleViewportsPtr));
552 
553  // rate
554  ViewportPointCloudPtr ratedSampleViewportsPtr;
555  mCalculator.rateViewports(feasibleSampleViewportsPtr, currentCameraViewport, ratedSampleViewportsPtr, request.use_object_type_to_rate);
556 
557  mDebugHelperPtr->write(std::stringstream() << "number of rated viewports: " << ratedSampleViewportsPtr->size(), DebugHelper::SERVICE_CALLS);
558 
559  // threads mix up oldIdx
560  std::sort(ratedSampleViewportsPtr->begin(), ratedSampleViewportsPtr->end(), [](ViewportPoint a, ViewportPoint b)
561  {
562  return a.oldIdx < b.oldIdx;
563  });
564 
565  // convert to response
566  unsigned int nextRatedViewportIdx = 0; // to iterate through ratedSampleViewportsPtr
567  unsigned int nextRatedViewportOldIdx = 0; // oldIdx attribute of next ratedSampleViewport/nextRatedViewportIdx
568  if (ratedSampleViewportsPtr->size() == 0) {
569  nextRatedViewportOldIdx = -1;
570  } else {
571  nextRatedViewportOldIdx = ratedSampleViewportsPtr->at(nextRatedViewportIdx).oldIdx;
572  }
573  for (i = 0; i < sampleViewportsPtr->size(); i++) {
574  asr_next_best_view::RatedViewport responseViewport;
575  if (i != nextRatedViewportOldIdx) {
576  ViewportPoint unratedViewport = sampleViewportsPtr->at(i);
577  responseViewport.pose = unratedViewport.getPose();
578  responseViewport.oldIdx = i;
579  if (unratedViewport.object_type_set->size() > 0) {
580  responseViewport.object_type_name_list = std::vector<std::string>(unratedViewport.object_type_set->size());
581  std::copy(unratedViewport.object_type_set->begin(),
582  unratedViewport.object_type_set->end(),
583  responseViewport.object_type_name_list.begin());
584  }
585  } else {
586  // assert(i == nextRatedViewportOldIdx) ^= ratedSampleViewportsPtr->at(nextRatedViewportIdx).oldIdx
587  ViewportPoint& ratedViewport = ratedSampleViewportsPtr->at(nextRatedViewportIdx);
588  responseViewport.pose = ratedViewport.getPose();
589  responseViewport.oldIdx = i;
590  if (ratedViewport.object_type_set->size() > 0) {
591  responseViewport.object_type_name_list = std::vector<std::string>(ratedViewport.object_type_set->size());
592  std::copy(ratedViewport.object_type_set->begin(),
593  ratedViewport.object_type_set->end(),
594  responseViewport.object_type_name_list.begin());
595  }
596  responseViewport.rating = mCalculator.getRatingModule()->getRating(ratedViewport.score);
597  // set utility and inverse cost terms in rating for the given next best view.
598  responseViewport.utility = ratedViewport.score->getUnweightedUnnormalizedUtility();
599  responseViewport.inverse_costs = ratedViewport.score->getWeightedInverseCosts();
600  responseViewport.base_translation_inverse_costs = ratedViewport.score->getUnweightedInverseMovementCostsBaseTranslation();
601  responseViewport.base_rotation_inverse_costs = ratedViewport.score->getUnweightedInverseMovementCostsBaseRotation();
602  responseViewport.ptu_movement_inverse_costs = ratedViewport.score->getUnweightedInverseMovementCostsPTU();
603  responseViewport.recognition_inverse_costs = ratedViewport.score->getUnweightedInverseRecognitionCosts();
604  nextRatedViewportIdx++;
605  if (nextRatedViewportIdx < ratedSampleViewportsPtr->size()) {
606  nextRatedViewportOldIdx = ratedSampleViewportsPtr->at(nextRatedViewportIdx).oldIdx;
607  }
608  }
609  response.sortedRatedViewports.push_back(responseViewport);
610  }
611 
612  // sort
613  std::stable_sort(response.sortedRatedViewports.begin(), response.sortedRatedViewports.end(), [](asr_next_best_view::RatedViewport a, asr_next_best_view::RatedViewport b)
614  {
615  return a.rating > b.rating;
616  });
617 
618  mDebugHelperPtr->writeNoticeably("ENDING NBV RATE-VIEWPORTS SERVICE CALL", DebugHelper::SERVICE_CALLS);
619  return true;
620  }
621 
622  bool processRemoveObjects(asr_next_best_view::RemoveObjects::Request &request, asr_next_best_view::RemoveObjects::Response &response) {
623 
624  mDebugHelperPtr->writeNoticeably("STARTING NBV REMOVE-OBJECTS SERVICE CALL", DebugHelper::SERVICE_CALLS);
625 
626  bool is_valid = mCalculator.removeObjects(request.type, request.identifier);
627  // if pointcloud is empty after this call
628  response.is_valid = is_valid;
629 
630  mDebugHelperPtr->writeNoticeably("ENDING NBV REMOVE-OBJECTS SERVICE CALL", DebugHelper::SERVICE_CALLS);
631 
632  return true;
633  }
634 
635  static void convertObjectPointCloudToAttributedPointCloud(const ObjectPointCloud &pointCloud, asr_msgs::AsrAttributedPointCloud &pointCloudMessage, uint minNumberNormals) {
636  pointCloudMessage.elements.clear();
637 
638  BOOST_FOREACH(ObjectPoint point, pointCloud) {
639 
640  // skip objects with too few normals
641  if (point.active_normal_vectors->size() < minNumberNormals)
642 // if (point.active_normal_vectors->size() < point.normal_vectors->size())
643  continue;
644 
645  asr_msgs::AsrAttributedPoint aPoint;
646 
647  aPoint.pose = point.getPose();
648  aPoint.type = point.type;
649  aPoint.identifier = point.identifier;
650 
651  pointCloudMessage.elements.push_back(aPoint);
652  }
653  }
654 
655  bool processGetPointCloudServiceCall(asr_next_best_view::GetAttributedPointCloud::Request &request, asr_next_best_view::GetAttributedPointCloud::Response &response) {
656  mDebugHelperPtr->writeNoticeably("STARTING NBV GET-POINT-CLOUD SERVICE CALL", DebugHelper::SERVICE_CALLS);
657 
658  // minNumberNormals defaults to 1 if not set
659  if (request.minNumberNormals < 1)
660  request.minNumberNormals = 1;
661  convertObjectPointCloudToAttributedPointCloud(*mCalculator.getPointCloudPtr(), response.point_cloud, request.minNumberNormals);
662 
663  mDebugHelperPtr->writeNoticeably("ENDING NBV GET-POINT-CLOUD SERVICE CALL", DebugHelper::SERVICE_CALLS);
664  return true;
665  }
666 
667  bool processSetPointCloudServiceCall(asr_next_best_view::SetAttributedPointCloud::Request &request, asr_next_best_view::SetAttributedPointCloud::Response &response) {
668 
669  mDebugHelperPtr->writeNoticeably("STARTING NBV SET-POINT-CLOUD SERVICE CALL", DebugHelper::SERVICE_CALLS);
670 
671  if (!mCalculator.setPointCloudFromMessage(request.point_cloud)) {
672  ROS_ERROR("Could not set point cloud from message.");
673  mDebugHelperPtr->writeNoticeably("ENDING NBV SET-POINT-CLOUD SERVICE CALL", DebugHelper::SERVICE_CALLS);
674  return false;
675  }
676 
677  if(mCalculator.getPointCloudPtr()->size() == 0)
678  {
679  response.is_valid = false;
680  mDebugHelperPtr->writeNoticeably("ENDING NBV SET-POINT-CLOUD SERVICE CALL", DebugHelper::SERVICE_CALLS);
681  return true;
682  }
683 
684  // convert to viewportPointCloud
685  std::vector<ViewportPoint> viewportPointList;
686  BOOST_FOREACH(asr_msgs::AsrViewport &viewport, request.viewports_to_filter)
687  {
688  ViewportPoint viewportConversionPoint(viewport.pose);
689  viewportConversionPoint.object_type_set = boost::shared_ptr<ObjectTypeSet>(new ObjectTypeSet());
690  viewportConversionPoint.object_type_set->insert(viewport.object_type_name_list.begin(), viewport.object_type_name_list.end());
691  viewportPointList.push_back(viewportConversionPoint);
692  }
693 
694  //Filter point cloud with those views.
695  mCalculator.updateFromExternalViewportPointList(viewportPointList);
696 
697  response.is_valid = true;
698 
699  // object ^= object type + identifier
700  std::vector<std::pair<std::string, std::string>> typeAndIds = mCalculator.getTypeAndIds();
701  for (auto &typeAndId : typeAndIds) {
702  asr_next_best_view::NormalsInfo curNormalsInfo;
703  std::string type = typeAndId.first;
704  std::string identifier = typeAndId.second;
705  unsigned int activeNormals = mCalculator.getNumberActiveNormals(type, identifier);
706  unsigned int totalNormals = mCalculator.getNumberTotalNormals(type, identifier);
707  unsigned int deactivatedNormals = totalNormals - activeNormals;
708  curNormalsInfo.active_normals = totalNormals;
709  curNormalsInfo.deactivated_object_normals = deactivatedNormals;
710  curNormalsInfo.type = type;
711  curNormalsInfo.identifier = identifier;
712  response.normals_per_object.push_back(curNormalsInfo);
713  }
714 
715  // publish the visualization
717  mDebugHelperPtr->writeNoticeably("ENDING NBV SET-POINT-CLOUD SERVICE CALL", DebugHelper::SERVICE_CALLS);
718  return true;
719 
720  }
721 
722  bool processSetInitRobotStateServiceCall(asr_next_best_view::SetInitRobotState::Request &request, asr_next_best_view::SetInitRobotState::Response &response) {
723  mDebugHelperPtr->writeNoticeably("STARTING NBV SET-INIT-ROBOT-STATE SERVICE CALL", DebugHelper::SERVICE_CALLS);
724 
725  robot_model_services::MILDRobotStatePtr currentRobotStatePtr(new robot_model_services::MILDRobotState());
726  currentRobotStatePtr->pan = request.robotState.pan;
727  currentRobotStatePtr->tilt = request.robotState.tilt;
728  currentRobotStatePtr->rotation = request.robotState.rotation;
729  currentRobotStatePtr->x = request.robotState.x;
730  currentRobotStatePtr->y = request.robotState.y;
731 
732  ROS_INFO_STREAM("Initial pose: pan " << currentRobotStatePtr->pan << ", tilt " << currentRobotStatePtr->tilt
733  << ", rotation " << currentRobotStatePtr->rotation << ", x " << currentRobotStatePtr->x << ", y " << currentRobotStatePtr->y);
734 
735  mCalculator.getRobotModel()->setCurrentRobotState(currentRobotStatePtr);
736 
737  mDebugHelperPtr->writeNoticeably("ENDING NBV SET-INIT-ROBOT-STATE SERVICE CALL", DebugHelper::SERVICE_CALLS);
738  return true;
739  }
740 
741  /* With this service call you can calculate the next next_best_view, given the last next_best_view.
742  */
743  bool processGetNextBestViewServiceCall(asr_next_best_view::GetNextBestView::Request &request, asr_next_best_view::GetNextBestView::Response &response) {
744  mDebugHelperPtr->writeNoticeably("STARTING NBV GET-NEXT-BEST-VIEW SERVICE CALL", DebugHelper::SERVICE_CALLS);
745  // Current camera view (frame of camera) of the robot.
746  ViewportPoint currentCameraViewport(request.current_pose);
747  //Contains Next best view.
748 
749  // we cannot find a nbv if pointcloud is empty
750  if (mCalculator.getPointCloudPtr()->empty()) {
751  response.found = false;
752  return true;
753  }
754 
755  ViewportPoint resultingViewport;
756  //Estimate the Next best view.
757  if (!mCalculator.calculateNextBestView(currentCameraViewport, resultingViewport)) {
758  //No points from input cloud in any nbv candidate or iterative search aborted (by user).
759  mDebugHelperPtr->write("No more next best view found.", DebugHelper::SERVICE_CALLS);
760  response.found = false;
761  mDebugHelperPtr->writeNoticeably("ENDING NBV GET-NEXT-BEST-VIEW SERVICE CALL", DebugHelper::SERVICE_CALLS);
762  return true;
763  }
764  //Return the optimization result including its parameters.
765  response.found = true;
766  response.viewport.pose = resultingViewport.getPose();
767 
768  // copying the objects to be searched for into a list
769  response.viewport.object_type_name_list = ObjectTypeList(resultingViewport.object_type_set->size());
770  std::copy(resultingViewport.object_type_set->begin(), resultingViewport.object_type_set->end(), response.viewport.object_type_name_list.begin());
771 
772  // parameters of the viewport
773  response.viewport.fovx = mConfig.fovx;
774  response.viewport.fovy = mConfig.fovy;
775  response.viewport.ncp = mConfig.ncp;
776  response.viewport.fcp = mConfig.fcp;
777 
778  //Reconstruct robot configuration for next best camera viewport (once known when estimating its costs) for outpout purposes.
779  // TODO: This solution is very dirty because we get the specialization of RobotState and this will break if we change the RobotModel and RobotState type.
780  robot_model_services::RobotStatePtr state = mCalculator.getRobotModel()->calculateRobotState(resultingViewport.getPosition(), resultingViewport.getSimpleQuaternion());
781  robot_model_services::MILDRobotStatePtr mildState = boost::static_pointer_cast<robot_model_services::MILDRobotState>(state);
782 
783  asr_next_best_view::RobotStateMessage robotStateMsg;
784  robotStateMsg.pan = mildState->pan;
785  robotStateMsg.tilt = mildState->tilt;
786  robotStateMsg.rotation = mildState->rotation;
787  robotStateMsg.x = mildState->x;
788  robotStateMsg.y = mildState->y;
789 
790  // set it to the response
791  response.robot_state = robotStateMsg;
792 
793  // set utility and inverse cost terms in rating for the given next best view.
794  response.utility = resultingViewport.score->getUnweightedUnnormalizedUtility();
795  response.utility_normalization = resultingViewport.score->getUtilityNormalization();
796  response.inverse_costs = resultingViewport.score->getWeightedInverseCosts();
797  response.base_translation_inverse_costs = resultingViewport.score->getUnweightedInverseMovementCostsBaseTranslation();
798  response.base_rotation_inverse_costs = resultingViewport.score->getUnweightedInverseMovementCostsBaseRotation();
799  response.ptu_movement_inverse_costs = resultingViewport.score->getUnweightedInverseMovementCostsPTU();
800  response.recognition_inverse_costs = resultingViewport.score->getUnweightedInverseRecognitionCosts();
801 
802  mCurrentCameraViewport = resultingViewport;
803 
804  mDebugHelperPtr->writeNoticeably("ENDING NBV GET-NEXT-BEST-VIEW SERVICE CALL", DebugHelper::SERVICE_CALLS);
805  return true;
806  }
807 
808  bool processUpdatePointCloudServiceCall(asr_next_best_view::UpdatePointCloud::Request &request,asr_next_best_view:: UpdatePointCloud::Response &response) {
809  mDebugHelperPtr->writeNoticeably("STARTING NBV UPDATE-POINT-CLOUD SERVICE CALL", DebugHelper::SERVICE_CALLS);
810 
811  // output
812  std::stringstream pose;
813  pose << "x = " << request.pose_for_update.position.x << ", "
814  << "y = " << request.pose_for_update.position.y << ", "
815  << "z = " << request.pose_for_update.position.z << ", "
816  << "qw = " << request.pose_for_update.orientation.w << ", "
817  << "qx = " << request.pose_for_update.orientation.x << ", "
818  << "qy = " << request.pose_for_update.orientation.y << ", "
819  << "qz = " << request.pose_for_update.orientation.z;
820  std::string objects = "";
821  BOOST_FOREACH(std::string object, request.object_type_name_list) {
822  if (objects.length() != 0) {
823  objects += ", ";
824  }
825  objects += object;
826  }
827 
828  //mDebugHelperPtr->write(std::stringstream() << "Updating with pose: " << pose, DebugHelper::SERVICE_CALLS);
829  mDebugHelperPtr->write(std::stringstream() << "Updating objects: " << objects, DebugHelper::SERVICE_CALLS);
830 
831  // convert data types
832  SimpleVector3 point = TypeHelper::getSimpleVector3(request.pose_for_update);
833  SimpleQuaternion orientation = TypeHelper::getSimpleQuaternion(request.pose_for_update);
834  ViewportPoint viewportPoint;
835  mDebugHelperPtr->write(std::stringstream() << "Do frustum culling: ActiveIndices="
836  << mCalculator.getActiveIndices()->size(),
838  if (!mCalculator.doFrustumCulling(point, orientation, mCalculator.getActiveIndices(), viewportPoint)) {
839  mDebugHelperPtr->write("no objects in frustum", DebugHelper::SERVICE_CALLS);
840  return true;
841  }
842  mDebugHelperPtr->write("Do update object point cloud", DebugHelper::SERVICE_CALLS);
843 
844  // copy objects to be updated from list to set
845  ObjectTypeSetPtr objectTypeSetPtr = ObjectTypeSetPtr(new ObjectTypeSet(request.object_type_name_list.begin(), request.object_type_name_list.end()));
846  unsigned int deactivatedNormals = mCalculator.updateObjectPointCloud(objectTypeSetPtr, viewportPoint);
847 
848  response.deactivated_object_normals = deactivatedNormals;
849 
850  this->publishPointCloudNormals();
851 
852  mDebugHelperPtr->writeNoticeably("ENDING NBV UPDATE-POINT-CLOUD SERVICE CALL", DebugHelper::SERVICE_CALLS);
853  return true;
854  }
855 
856  bool processTriggerFrustumVisualization(asr_next_best_view::TriggerFrustumVisualization::Request &request,
857  asr_next_best_view::TriggerFrustumVisualization::Response &response)
858  {
859  mDebugHelperPtr->writeNoticeably("STARTING NBV TRIGGER-FRUSTUM-VISUALIZATION SERVICE CALL", DebugHelper::SERVICE_CALLS);
860  geometry_msgs::Pose pose = request.current_pose;
863  mCalculator.getCameraModelFilter()->setPivotPointPose(position, orientation);
864 
865  mVisHelperPtr->triggerNewFrustumVisualization(mCalculator.getCameraModelFilter());
866 
867  mDebugHelperPtr->writeNoticeably("ENDING NBV TRIGGER-FRUSTUM-VISUALIZATION SERVICE CALL", DebugHelper::SERVICE_CALLS);
868  return true;
869  }
870 
871  bool processTriggerOldFrustumVisualization(asr_next_best_view::TriggerFrustumVisualization::Request &request,
872  asr_next_best_view::TriggerFrustumVisualization::Response &response)
873  {
874  mDebugHelperPtr->writeNoticeably("STARTING NBV TRIGGER-OLD-FRUSTUM-VISUALIZATION SERVICE CALL", DebugHelper::SERVICE_CALLS);
875  geometry_msgs::Pose pose = request.current_pose;
878  mCalculator.getCameraModelFilter()->setPivotPointPose(position, orientation);
879 
880  mVisHelperPtr->triggerOldFrustumVisualization(this->mCalculator.getCameraModelFilter());
881 
882  mDebugHelperPtr->writeNoticeably("ENDING NBV TRIGGER-OLD-FRUSTUM-VISUALIZATION SERVICE CALL", DebugHelper::SERVICE_CALLS);
883  return true;
884  }
885 
886  bool processTriggerFrustumsAndPointCloudVisualization(asr_next_best_view::TriggerFrustumsAndPointCloudVisualization::Request &request,
887  asr_next_best_view::TriggerFrustumsAndPointCloudVisualization::Response &response)
888  {
889  mDebugHelperPtr->writeNoticeably("STARTING NBV TRIGGER-FRUSTUMS-AND-POINT-CLOUD-VISUALIZATION SERVICE CALL", DebugHelper::SERVICE_CALLS);
890 
891  // set viewport
892  ViewportPoint viewport(request.new_viewport.pose);
893 
894  IndicesPtr childIndicesPtr;
895  if (!mCalculator.getFeasibleHypothesis(viewport.getPosition(), childIndicesPtr))
896  {
897  viewport.child_indices = IndicesPtr(new Indices());
898  triggerVisualization(viewport);
899  return true;
900  }
901 
902  viewport.child_indices = childIndicesPtr;
903 
904  if (!mCalculator.doFrustumCulling(viewport))
905  {
906  triggerVisualization(viewport);
907  return true;
908  }
909 
910  ObjectTypeSetPtr objectTypeSetPtr = ObjectTypeSetPtr(new ObjectTypeSet());
911 
912  BOOST_FOREACH(std::string objectType, request.new_viewport.object_type_name_list)
913  objectTypeSetPtr->insert(objectType);
914 
915  ViewportPoint filteredViewport;
916  viewport.filterObjectPointCloudByTypes(objectTypeSetPtr, filteredViewport);
917 
918  // visualize viewport
919  triggerVisualization(filteredViewport);
920 
921  mDebugHelperPtr->writeNoticeably("ENDING NBV TRIGGER-FRUSTUMS-AND-POINT-CLOUD-VISUALIZATION SERVICE CALL", DebugHelper::SERVICE_CALLS);
922 
923  return true;
924  }
925 
927  return this->triggerVisualization(mCurrentCameraViewport);
928  }
929 
931  if (mCurrentlyPublishingVisualization) {
932  mDebugHelperPtr->write("Already generating visualization data.", DebugHelper::VISUALIZATION);
933  return false;
934  }
935 
936  mCurrentlyPublishingVisualization = true;
937 
938  this->mVisualizationThread = boost::shared_ptr<boost::thread>(new boost::thread(&NextBestView::publishVisualization, this, viewport, true));
939  return true;
940  }
941 
946  void publishVisualization(ViewportPoint viewport, bool publishFrustum) {
947  mDebugHelperPtr->write("Publishing Visualization with viewport", DebugHelper::VISUALIZATION);
948 
949  mCalculator.getCameraModelFilter()->setPivotPointPose(viewport.getPosition(), viewport.getSimpleQuaternion());
950 
951  mDebugHelperPtr->write(std::stringstream() << "Frustum Pivot Point : " << mCalculator.getCameraModelFilter()->getPivotPointPosition()[0]
952  << " , " << mCalculator.getCameraModelFilter()->getPivotPointPosition()[1]
953  << " , " << mCalculator.getCameraModelFilter()->getPivotPointPosition()[2],
955 
956  mDebugHelperPtr->write(std::stringstream() << "Object points in the viewport: " << viewport.child_indices->size(),
958 
959  if (mShowPointcloud)
960  {
961  // publish object point cloud
962  Indices pointCloudIndices;
963  if (mShowFrustumPointCloud) {
964  // if the frustum point cloud is published seperately only publish points outside frustum
965  this->getIndicesOutsideFrustum(viewport, pointCloudIndices);
966  } else {
967  pointCloudIndices = *mCalculator.getActiveIndices();
968  }
969 
970  ObjectPointCloud objectPointCloud = ObjectPointCloud(*mCalculator.getPointCloudPtr(), pointCloudIndices);
971  std::map<std::string, std::string> typeToMeshResource = this->getMeshResources(objectPointCloud);
972 
973  mVisHelperPtr->triggerObjectPointCloudVisualization(objectPointCloud, typeToMeshResource);
974  }
975  if (mShowFrustumPointCloud)
976  {
977  // publish frustum object point cloud
978  ObjectPointCloud frustumObjectPointCloud = ObjectPointCloud(*mCalculator.getPointCloudPtr(), *viewport.child_indices);
979  std::map<std::string, std::string> typeToMeshResource = this->getMeshResources(frustumObjectPointCloud);
980 
981  mVisHelperPtr->triggerFrustumObjectPointCloudVisualization(frustumObjectPointCloud, typeToMeshResource);
982  }
983  if (mShowFrustumMarkerArray && publishFrustum)
984  {
985  // publish furstums visualization
986  mVisHelperPtr->triggerFrustumsVisualization(this->mCalculator.getCameraModelFilter());
987  }
988  mCurrentlyPublishingVisualization = false;
989  }
990 
992  mDebugHelperPtr->write("Publishing visualization of new point cloud", DebugHelper::VISUALIZATION);
993 
994  ObjectPointCloud objectPointCloud = ObjectPointCloud(*mCalculator.getPointCloudPtr(), *mCalculator.getActiveIndices());
995 
996  if (mShowPointcloud)
997  {
998  // clear visualization of old objects in frustum
999  mVisHelperPtr->clearFrustumObjectPointCloudVisualization();
1000 
1001  // publish new object point cloud
1002  std::map<std::string, std::string> typeToMeshResource = this->getMeshResources(objectPointCloud);
1003 
1004  mVisHelperPtr->triggerObjectPointCloudVisualization(objectPointCloud, typeToMeshResource);
1005  }
1006 
1007  if (mShowNormals) {
1008  // publish new normals
1009  mVisHelperPtr->triggerObjectNormalsVisualization(objectPointCloud);
1010  }
1011  }
1012 
1014  mDebugHelperPtr->write("Publishing normals", DebugHelper::VISUALIZATION);
1015 
1016  if (mShowNormals) {
1017  // show normals
1018  ObjectPointCloud objectPointCloud = ObjectPointCloud(*mCalculator.getPointCloudPtr(), *mCalculator.getActiveIndices());
1019  mVisHelperPtr->triggerObjectNormalsVisualization(objectPointCloud);
1020  }
1021  }
1022 
1024  return mCalculator;
1025  }
1026 
1027  void getIndicesOutsideFrustum(const ViewportPoint &viewport, Indices &resultIndices) {
1028  IndicesPtr activeIndices = mCalculator.getActiveIndices();
1029  IndicesPtr childIndices = viewport.child_indices;
1030  std::set_difference(activeIndices->begin(), activeIndices->end(),
1031  childIndices->begin(), childIndices->end(),
1032  std::back_inserter(resultIndices));
1033  }
1034 
1035  std::map<std::string, std::string> getMeshResources(ObjectPointCloud objectPointCloud) {
1036  std::map<std::string, std::string> typeToMeshResource;
1037  for(ObjectPointCloud::iterator it = objectPointCloud.begin(); it < objectPointCloud.end(); it++) {
1038  if (typeToMeshResource.count(it->type) == 0) {
1039  std::string path = mCalculator.getMeshPathByName(it->type);
1040  // check if the path is valid
1041  if (path.compare("-2") != 0) {
1042  typeToMeshResource[it->type] = mCalculator.getMeshPathByName(it->type);
1043  }
1044  }
1045  }
1046  return typeToMeshResource;
1047  }
1048 
1049  void dynamicReconfigureCallback(asr_next_best_view::DynamicParametersConfig &config, uint32_t level) {
1050  // TODO consider that services and this and other stuff is called parallel
1051  ROS_INFO_STREAM("Parameters updated");
1052  ROS_INFO_STREAM("level: " << level);
1053  this->mConfig = config;
1054  this->mConfigLevel = level;
1055  initialize();
1056  }
1057 };
1059 }
1060 
bool rateViewports(const ViewportPointCloudPtr &sampleNextBestViewportsPtr, const ViewportPoint &currentCameraViewport, ViewportPoint &resultViewport, bool objectTypeSetIsKnown=false)
rates given viewports, which must contain child_indices
bool getFeasibleHypothesis(SimpleVector3 samplePoint, IndicesPtr &resultIndicesPtr)
getFeasibleHypothesis finds object hypothesis near samplePoint
bool processTriggerOldFrustumVisualization(asr_next_best_view::TriggerFrustumVisualization::Request &request, asr_next_best_view::TriggerFrustumVisualization::Response &response)
VisualizationHelperPtr mVisHelperPtr
void setHypothesisUpdater(const HypothesisUpdaterPtr &hypothesisUpdaterPtr)
unsigned int getNumberTotalNormals(std::string type, std::string identifier)
bool processGetNextBestViewServiceCall(asr_next_best_view::GetNextBestView::Request &request, asr_next_best_view::GetNextBestView::Response &response)
CameraModelFilterAbstractFactoryPtr createCameraModelFromConfig(int moduleId)
pcl::PointCloud< ObjectPoint > ObjectPointCloud
Definition: typedef.hpp:85
void setEnableIntermediateObjectWeighting(const bool mEnableIntermediateObjectWeighting)
HypothesisUpdaterAbstractFactoryPtr createHypothesisUpdaterFromConfig(int moduleId)
bool doFrustumCulling(const SimpleVector3 &position, const SimpleQuaternion &orientation, const IndicesPtr &indices, ViewportPoint &viewportPoint)
creates a new camera viewport with the given data
RatingModuleAbstractFactoryPtr createRatingModuleFromConfig(int moduleId)
void setCameraModelFilter(const CameraModelFilterPtr &cameraModelFilterPtr)
f
ros::ServiceServer mResetCalculatorServer
bool processResetCalculatorServiceCall(asr_next_best_view::ResetCalculator::Request &request, asr_next_best_view::ResetCalculator::Response &response)
std::string getMeshPathByName(std::string objectType)
asr_next_best_view::DynamicParametersConfig mConfig
void setCameraModelFilterAbstractFactoryPtr(const CameraModelFilterAbstractFactoryPtr &cameraModelFilterAbstractFactoryPtr)
setCameraModelFilterAbstractFactoryPtr used to generate camera models per thread. ...
ViewportPointCloud::Ptr ViewportPointCloudPtr
Definition: typedef.hpp:97
Eigen::Matrix< Precision, 3, 1 > SimpleVector3
Definition: typedef.hpp:53
boost::shared_ptr< UnitSphereSamplerAbstractFactory > UnitSphereSamplerAbstractFactoryPtr
void publishVisualization(ViewportPoint viewport, bool publishFrustum)
Publishes the Visualization of the NextBestView.
ros::ServiceServer mSetPointCloudServiceServer
void initializeRobotState(const ViewportPoint &currentCameraViewport)
initializeRobotState initializes the robotstate to the given viewport
bool setPointCloudFromMessage(const asr_msgs::AsrAttributedPointCloud &msg)
///
robot_model_services::RobotModelAbstractFactoryPtr mRobotModelFactoryPtr
bool processTriggerFrustumVisualization(asr_next_best_view::TriggerFrustumVisualization::Request &request, asr_next_best_view::TriggerFrustumVisualization::Response &response)
ros::ServiceServer mRemoveObjectsServer
boost::shared_ptr< SpaceSamplerAbstractFactory > SpaceSamplerAbstractFactoryPtr
ROSCPP_DECL const std::string & getName()
NextBestView is a configuration class of the related node.
ViewportPoint mCurrentCameraViewport
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool processSetInitRobotStateServiceCall(asr_next_best_view::SetInitRobotState::Request &request, asr_next_best_view::SetInitRobotState::Response &response)
void setNumberOfThreads(int value)
sets number of threads used to rate samples, negative numbers use boost::thread::hardware_concurrency...
std::set< std::string > ObjectTypeSet
Definition: typedef.hpp:124
std::vector< int > Indices
Definition: typedef.hpp:117
boost::shared_ptr< boost::thread > mVisualizationThread
ros::NodeHandle mGlobalNodeHandle
unsigned int updateObjectPointCloud(const ObjectTypeSetPtr &objectTypeSetPtr, const ViewportPoint &viewportPoint)
Updates the point cloud under the assumption that the given viewport was chosen.
bool triggerVisualization(ViewportPoint viewport)
SimpleQuaternion getSimpleQuaternion() const
void dynamicReconfigureCallback(asr_next_best_view::DynamicParametersConfig &config, uint32_t level)
ros::ServiceServer mGetNextBestViewServiceServer
void setSpaceSampler(const SpaceSamplerPtr &spaceSamplerPtr)
ros::ServiceServer mSetInitRobotStateServiceServer
boost::shared_ptr< HypothesisUpdaterAbstractFactory > HypothesisUpdaterAbstractFactoryPtr
this namespace contains all generally usable classes.
void setRatingModuleAbstractFactoryPtr(const RatingModuleAbstractFactoryPtr &ratingModuleAbstractFactoryPtr)
setRatingModuleAbstractFactoryPtr used to generate rating modules per thread.
void setMapHelper(const MapHelperPtr &mapHelperPtr)
NextBestViewCalculator mCalculator
UnitSphereSamplerAbstractFactoryPtr mSphereSamplerFactoryPtr
pcl::PointCloud< ViewportPoint > ViewportPointCloud
Definition: typedef.hpp:96
UnitSphereSamplerAbstractFactoryPtr createSphereSamplerFromConfig(int moduleId)
void loadCropBoxListFromFile(const std::string mCropBoxListFilePath)
static boost::shared_ptr< DebugHelper > getInstance()
Definition: DebugHelper.cpp:29
void updateFromExternalViewportPointList(const std::vector< ViewportPoint > &viewportPointList)
updates point cloud with external viewport point list
boost::shared_ptr< Indices > IndicesPtr
Definition: typedef.hpp:118
bool filterObjectPointCloudByTypes(const ObjectTypeSetPtr &objectTypeSetPtr, ViewportPoint &viewportPoint)
filters all the objects in this viewport with one of the given types and puts them in a new viewport...
void setUnitSphereSampler(const UnitSphereSamplerPtr &unitSphereSamplerPtr)
ros::ServiceServer mUpdatePointCloudServiceServer
SpaceSamplerAbstractFactoryPtr mSpaceSampleFactoryPtr
bool processUpdatePointCloudServiceCall(asr_next_best_view::UpdatePointCloud::Request &request, asr_next_best_view::UpdatePointCloud::Response &response)
boost::shared_ptr< CameraModelFilterAbstractFactory > CameraModelFilterAbstractFactoryPtr
boost::shared_ptr< ObjectTypeSet > ObjectTypeSetPtr
Definition: typedef.hpp:125
RatingModuleAbstractFactoryPtr mRatingModuleFactoryPtr
boost::shared_ptr< RatingModuleAbstractFactory > RatingModuleAbstractFactoryPtr
boost::shared_ptr< VisualizationHelper > VisualizationHelperPtr
bool processTriggerFrustumsAndPointCloudVisualization(asr_next_best_view::TriggerFrustumsAndPointCloudVisualization::Request &request, asr_next_best_view::TriggerFrustumsAndPointCloudVisualization::Response &response)
robot_model_services::RobotModelPtr getRobotModel()
void setEnableCropBoxFiltering(const bool mEnableCropBoxFiltering)
robot_model_services::RobotModelAbstractFactoryPtr createRobotModelFromConfig(int moduleId)
ros::ServiceServer mTriggerFrustumVisualizationServer
HypothesisUpdaterAbstractFactoryPtr mHypothesisUpdaterFactoryPtr
std::vector< std::pair< std::string, std::string > > getTypeAndIds()
bool removeObjects(std::string type, std::string identifier)
#define ROS_INFO_STREAM(args)
void setRobotModel(const robot_model_services::RobotModelPtr &robotModelPtr)
static void convertObjectPointCloudToAttributedPointCloud(const ObjectPointCloud &pointCloud, asr_msgs::AsrAttributedPointCloud &pointCloudMessage, uint minNumberNormals)
ros::ServiceServer mGetPointCloudServiceServer
NextBestView()
Creates an instance of the NextBestView class.
bool processGetPointCloudServiceCall(asr_next_best_view::GetAttributedPointCloud::Request &request, asr_next_best_view::GetAttributedPointCloud::Response &response)
bool getParam(const std::string &key, std::string &s) const
static SimpleVector3 getSimpleVector3(const geometry_msgs::Pose &pose)
Definition: TypeHelper.cpp:44
bool processRateViewports(asr_next_best_view::RateViewports::Request &request, asr_next_best_view::RateViewports::Response &response)
bool processSetPointCloudServiceCall(asr_next_best_view::SetAttributedPointCloud::Request &request, asr_next_best_view::SetAttributedPointCloud::Response &response)
dynamic_reconfigure::Server< asr_next_best_view::DynamicParametersConfig > mDynamicReconfigServer
std::map< std::string, std::string > getMeshResources(ObjectPointCloud objectPointCloud)
bool calculateNextBestView(const ViewportPoint &currentCameraViewport, ViewportPoint &resultViewport)
static SimpleQuaternion getSimpleQuaternion(const geometry_msgs::Pose &pose)
Definition: TypeHelper.cpp:64
#define ROS_ERROR_STREAM(args)
void getIndicesOutsideFrustum(const ViewportPoint &viewport, Indices &resultIndices)
Eigen::Quaternion< Precision > SimpleQuaternion
Definition: typedef.hpp:67
ros::ServiceServer mTriggerOldFrustumVisualizationServer
bool processRemoveObjects(asr_next_best_view::RemoveObjects::Request &request, asr_next_best_view::RemoveObjects::Response &response)
ros::ServiceServer mRateViewportsServer
boost::shared_ptr< asr_next_best_view::DynamicParametersConfig > DynamicParametersConfigPtr
#define ROS_ERROR(...)
NextBestViewCalculator & getCalculator()
ros::ServiceServer mTriggerFrustmsAndPointCloudVisualizationServer
void getFeasibleViewports(const ViewportPointCloudPtr &sampleViewportsPtr, IndicesPtr &resultIndicesPtr)
getFeasibleViewports returns a vector of indices/sampleViewports which contain nearby (hypothesis) ...
void setRatingModule(const RatingModulePtr &ratingModulePtr)
CameraModelFilterAbstractFactoryPtr mCameraModelFactoryPtr
std::vector< std::string > ObjectTypeList
Definition: typedef.hpp:122
boost::shared_ptr< MapHelper > MapHelperPtr
Definition: MapHelper.hpp:424
SpaceSamplerAbstractFactoryPtr createSpaceSamplerFromConfig(int moduleId)


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