grid_initialisation.cpp
Go to the documentation of this file.
1 
18 #include "grid_initialisation.hpp"
19 
20 namespace directSearchWS {
21 
22 GridInitialisation::GridInitialisation(std::string gridFilePath)
23  : DirectSearchHandler(), gridFilePath(gridFilePath), ptuTuplePtrVecPtr(), clearVision(0.0) {
24 }
25 
27 
30  double panMin, panMax;
31  double tiltMin, tiltMax;
32  double fovH, fovV;
33  nh.getParam("pan_min_angle", panMin);
34  nh.getParam("pan_max_angle", panMax);
35  nh.getParam("tilt_min_angle", tiltMin);
36  nh.getParam("tilt_max_angle", tiltMax);
37  nh.getParam("clearVision", clearVision);
38  nh.getParam("fovH", fovH);
39  nh.getParam("fovV", fovV);
40  ROS_DEBUG_STREAM("We got params panMin:" << panMin << ", panMax: " << panMax << ", tiltMin: " << tiltMin << ", tiltMax: " << tiltMax << ", fovH: " << fovH << ", fovV :" <<fovV );
41  double rangeV = tiltMax - tiltMin;
42  if (panMin < -clearVision) {
43  panMin = -clearVision;
44  }
45  if (panMax > clearVision) {
46  panMax = clearVision;
47  }
48  double rangeH = panMax - panMin;
49  int countH = ceil(rangeH/fovH);
50  int countV = ceil(rangeV/fovV);
51  double stepH = rangeH/countH;
52  double stepV = rangeV/countV;
53  ROS_DEBUG_STREAM("We calculted stepH: " << stepH << ", stepV: " << stepV << ", rangeV: "<< rangeV << ", rangeH: " << rangeH << ", countV: " << countV << ", countH: "<< countH);
54 
56  for (int i=1;i<=countH;i++) {
57  for (int j=1;j<=countV;j++) {
58  ptuTuplePtrVecPtr->push_back(PtuTuplePtr(new PtuTuple(panMin+i*stepH, tiltMin+j*stepV, nullptr)));
59  ROS_DEBUG_STREAM("PTU tuple created : "<< panMin+i*stepH << " , " <<tiltMin+j*stepV);
60  }
61  }
62 }
63 
65  bool success = DirectSearchHandler::resetHandler();
66  ptuTuplePtrVecPtr->clear();
67  success &= initHandler();
68  return success;
69 }
70 
71 
73  initPTUPoses();
74  //read config file.
75  // Read the xml file into a vector
76  std::ifstream ConfigFile(gridFilePath.c_str(),std::ifstream::in);
77  if (!ConfigFile) {
78  ROS_ERROR_STREAM("Unable to locate " << gridFilePath << ". Please make sure the launch file is correct and start again");
79  return false;
80  }
81  std::vector<char> buffer((std::istreambuf_iterator<char>(ConfigFile)), std::istreambuf_iterator<char>());
82  buffer.push_back('\0');
83  // Parse the buffer using the xml file parsing library into doc
84  rapidxml::xml_document<> doc;
85  doc.parse<0>(&buffer[0]);
86 
87  std::vector<geometry_msgs::Pose> orderedGrid;
88  // Find our root node
89  rapidxml::xml_node<> * root_node = doc.first_node("Posen");
90  for (rapidxml::xml_node<> * pose_node = root_node->first_node("Pose"); pose_node; pose_node = pose_node->next_sibling()) {
91  double X = boost::lexical_cast<double>((pose_node->first_attribute("X")->value()));
92  double Y = boost::lexical_cast<double>((pose_node->first_attribute("Y")->value()));
93  double Z = boost::lexical_cast<double>((pose_node->first_attribute("Z")->value()));
94  geometry_msgs::Pose pose;
95  pose.position.x = X;
96  pose.position.y = Y;
97  pose.position.z = Z;
98  orderedGrid.push_back(pose);
99  ROS_DEBUG_STREAM("Got Pose from XML X : " << X << ", Y : " << Y << ", Z : " << Z);
100 
101  }
102 
103  int rotationPerPose = round(180/(clearVision*2));
104  clearVision = 180/(rotationPerPose*2);
105 
106  geometry_msgs::Pose robotPose = poseHelperPtr->getCurrentRobotPose();
107  double dist = DBL_MAX;
108  auto iter = orderedGrid.begin();
109  for (auto it = orderedGrid.begin(); it != orderedGrid.end(); ++it) {
110  double currentDist = poseHelperPtr->calculateDistance(*it, robotPose);
111  if (currentDist < dist) {
112  dist = currentDist;
113  iter = it;
114  }
115  }
116  std::vector<geometry_msgs::Pose> temp;
117  if (iter != orderedGrid.begin()) {
118  for (auto it = iter; it != orderedGrid.end(); ++it) {
119  temp.push_back(*it);
120  }
121  for (auto it = orderedGrid.begin(); it != iter; ++it) {
122  temp.push_back(*it);
123  }
124  orderedGrid = temp;
125  }
126  temp.clear();
127  //myvector.emplace ( myvector.begin(), 100 );
128  //RobotState(Pose,1,std::make_tuple(0,0));
129 
130  std::vector<geometry_msgs::Pose> returnPath = orderedGrid;
131  std::reverse(returnPath.begin(), returnPath.end());
132  double yaw;
133  std::vector<double> yawArray;
134  ROS_DEBUG_STREAM("return size " << returnPath.size());
135  ROS_DEBUG_STREAM("size " << orderedGrid.size());
136  ROS_DEBUG_STREAM("Begin calculation for ordered grid");
137  for (auto it = orderedGrid.begin(); it != orderedGrid.end(); ++it) {
138  if (it != orderedGrid.end()-1) {
139  yaw = getYaw(it->position,(it+1)->position);
140  ROS_DEBUG_STREAM("got yaw : " << yaw*57);
141  } else {
142  yaw = getYaw(it->position,(orderedGrid.front()).position);
143  ROS_DEBUG_STREAM("got yaw : " << yaw*57);
144  }
145  yawArray.push_back(yaw);
146  for (int i=0;i<rotationPerPose;i++) {
147  geometry_msgs::Pose robotPose;
148  geometry_msgs::Quaternion quat= tf::createQuaternionMsgFromYaw(yaw+((i+1)*(clearVision*2))*M_PI/180);
149  robotPose.orientation = quat;
150  robotPose.position = it->position;
151  temp.push_back(robotPose);
152  }
153  }
154  ROS_DEBUG_STREAM("Begin calculation for return grid");
155  for (auto it = returnPath.begin(); it != returnPath.end(); ++it) {
156  if (yawArray.back() > M_PI) {
157  yaw = yawArray.back() - M_PI;
158  ROS_DEBUG_STREAM("got yaw : " << yaw*57);
159  } else {
160  yaw = yawArray.back() + M_PI;
161  ROS_DEBUG_STREAM("got yaw : " << yaw*57);
162  }
163  yawArray.pop_back();
164  for (int i=0;i<rotationPerPose;i++) {
165  geometry_msgs::Pose robotPose;
166  geometry_msgs::Quaternion quat= tf::createQuaternionMsgFromYaw(yaw+((i+1)*(clearVision*2))*M_PI/180);
167  robotPose.orientation = quat;
168  robotPose.position = it->position;
169  temp.push_back(robotPose);
170  }
171  }
172  orderedGrid = temp;
173 
174  for (auto it = orderedGrid.begin()+1; it != orderedGrid.end(); ++it) {
175  remainingPosesDistances += poseHelperPtr->calculateDistance(*it,*(it-1));
176  }
177 
179  for (const geometry_msgs::Pose &pose : orderedGrid) {
181  for (const PtuTuplePtr &tuple : *ptuTuplePtrVecPtr) {
182  ptuList->push_back(tuple);
183  }
184 
185  PosePtr posePtr = boost::make_shared<geometry_msgs::Pose>(pose);
186  posesToExplorePtr->push_back(RobotStatePtr(
187  new RobotState(
188  posePtr,
189  ptuList)));
190  }
191  return true;
192 }
193 
194 double GridInitialisation::getYaw(const geometry_msgs::Point &pointA, const geometry_msgs::Point &pointB) const {
195  double yaw = atan2((pointB.y-pointA.y),(pointB.x-pointA.x));
196  if ( yaw < 0.0f ) {
197  yaw += 2.0f * M_PI - (90+this->clearVision)*M_PI/180;
198  }
199  return yaw;
200 }
201 
202 }
203 
GridInitialisation(std::string gridFilePath)
f
boost::shared_ptr< RobotStatePtrVec > RobotStatePtrVecPtr
Definition: robot_state.hpp:52
ROSCPP_DECL const std::string & getName()
boost::shared_ptr< PtuTuple > PtuTuplePtr
Definition: ptu_tuple.hpp:69
double getYaw(const geometry_msgs::Point &pointA, const geometry_msgs::Point &pointB) const
std::vector< PtuTuplePtr > PtuTuplePtrVec
Definition: ptu_tuple.hpp:70
std::vector< RobotStatePtr > RobotStatePtrVec
Definition: robot_state.hpp:51
#define ROS_DEBUG_STREAM(args)
yaw
Definition: test.py:59
boost::shared_ptr< PtuTuplePtrVec > PtuTuplePtrVecPtr
Definition: ptu_tuple.hpp:71
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
bool getParam(const std::string &key, std::string &s) const
boost::shared_ptr< RobotState > RobotStatePtr
Definition: robot_state.hpp:50
#define ROS_ERROR_STREAM(args)


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