sort_helper.cpp
Go to the documentation of this file.
1 
18 #include "helper/sort_helper.hpp"
19 
20 namespace directSearchWS {
21 
22 SortHelper::SortHelper(RobotStatePtrVecPtr &posesToExplorePtr, bool needreorderPosesByTSP) :
23  poseHelperPtr(PoseHelper::getInstance()), posesToExplorePtr(posesToExplorePtr), needreorderPosesByTSP(needreorderPosesByTSP) {
24 }
25 
27  if (!needreorderPosesByTSP) {
28  return;
29  }
30  ROS_INFO("nearest_neighbour started");
31  if (posesToExplorePtr->size() < 3) {
32  ROS_DEBUG("There are less than 3 posesToExplore -> no optimization possible");
33  return;
34  }
35 
36  ros::Time begin = ros::Time::now();
37 
38  int numberOfSwaps = 0;
39  for (RobotStatePtrVec::iterator mainRobotIt = posesToExplorePtr->begin(); mainRobotIt != posesToExplorePtr->end() - 2; ++mainRobotIt) {
40  double currentBestDistance = DBL_MAX;
41  RobotStatePtrVec::iterator currentBestRobotIt;
42  for (RobotStatePtrVec::iterator robotIt = mainRobotIt + 1; robotIt != posesToExplorePtr->end(); ++robotIt) {
43  double distance = poseHelperPtr->calculateDistance(*mainRobotIt->get()->getRobotPosePtr(), *robotIt->get()->getRobotPosePtr());
44  if (distance < currentBestDistance) {
45  currentBestDistance = distance;
46  currentBestRobotIt = robotIt;
47  }
48  }
49  std::iter_swap(mainRobotIt + 1, currentBestRobotIt);
50  distance += currentBestDistance;
51  ++numberOfSwaps;
52  }
53  distance += poseHelperPtr->calculateDistance(*(posesToExplorePtr->end() - 2)->get()->getRobotPosePtr(),
54  *(posesToExplorePtr->end() - 1)->get()->getRobotPosePtr());
55  ROS_INFO_STREAM("nearest_neighbour finished. It took: " << (ros::Time::now() - begin).toSec() << "s, with numberOfSwaps: " << numberOfSwaps);
56 }
57 
59  if (!needreorderPosesByTSP) {
60  return;
61  }
62  ROS_INFO("two_opt started");
63  if (posesToExplorePtr->size() < 4) {
64  ROS_DEBUG("There are less than 4 posesToExplore -> no optimization possible");
65  return;
66  }
67  ROS_INFO_STREAM("distance before two_opt: " << distance);
68  ros::Time begin = ros::Time::now();
69 
70  int numberOfSwaps = 0;
71  bool improve;
72  do {
73  improve = false;
74  for (RobotStatePtrVec::iterator robotIt1 = posesToExplorePtr->begin(); robotIt1 != posesToExplorePtr->end() - 3; ++robotIt1) {
75  double bestChange = 0;
76  RobotStatePtrVec::iterator swap1;
77  RobotStatePtrVec::iterator swap2;
78  for (RobotStatePtrVec::iterator robotIt2 = robotIt1 + 2; robotIt2 != posesToExplorePtr->end() - 1; ++robotIt2) {
79  double change = poseHelperPtr->calculateDistance(*robotIt1->get()->getRobotPosePtr(), *robotIt2->get()->getRobotPosePtr())
80  + poseHelperPtr->calculateDistance(*(robotIt1 + 1)->get()->getRobotPosePtr(), *(robotIt2 + 1)->get()->getRobotPosePtr())
81  - poseHelperPtr->calculateDistance(*robotIt1->get()->getRobotPosePtr(), *(robotIt1 + 1)->get()->getRobotPosePtr())
82  - poseHelperPtr->calculateDistance(*robotIt2->get()->getRobotPosePtr(), *(robotIt2 + 1)->get()->getRobotPosePtr());
83  if (change < bestChange) {
84  bestChange = change;
85  swap1 = robotIt1 + 1;
86  swap2 = robotIt2;
87  improve = true;
88  }
89  }
90  if (bestChange < 0) {
91  distance += bestChange;
92  // Reverse [begin, end)
93  std::reverse(swap1, (swap2 + 1));
94  ++numberOfSwaps;
95  }
96  }
97  } while (improve);
98  ROS_INFO_STREAM("two_opt finished. It took: " << (ros::Time::now() - begin).toSec() << "s, with numberOfSwaps: " << numberOfSwaps);
99  ROS_INFO_STREAM("distance after two_opt: " << distance);
100 }
101 
102 }
103 
104 
105 
106 
RobotStatePtrVecPtr & posesToExplorePtr
Definition: sort_helper.hpp:31
SortHelper(RobotStatePtrVecPtr &posesToExplorePtr, bool needreorderPosesByTSP)
Definition: sort_helper.cpp:22
#define ROS_INFO(...)
#define ROS_INFO_STREAM(args)
static Time now()
void nearest_neighbour_and_update_distance(double &distance)
Definition: sort_helper.cpp:26
void two_opt_and_update_distance(double &distance)
Definition: sort_helper.cpp:58
#define ROS_DEBUG(...)


asr_direct_search_manager
Author(s): Borella Jocelyn, Karrenbauer Oliver, Mei├čner Pascal
autogenerated on Wed Jan 8 2020 03:15:41