main.cpp
Go to the documentation of this file.
1 
18 #include <boost/shared_ptr.hpp>
19 
20 #include <ros/ros.h>
22 
23 #include <geometry_msgs/Pose.h>
24 #include <asr_direct_search_manager/directSearchAction.h>
25 #include <dynamic_reconfigure/server.h>
26 
27 #include <asr_direct_search_manager/DynamicParametersConfig.h>
28 
30 #include "grid_initialisation.hpp"
31 #include "grid_manager.hpp"
32 #include "recording_manager.hpp"
33 #include "model/ptu_tuple.hpp"
34 
35 
36 namespace directSearchWS {
37 
39 private:
40 
41  boost::mutex mtx_;
43  // NodeHandle instance must be created before this line. Otherwise strange error may occur.
45  // create messages that are used to published feedback/result
46  asr_direct_search_manager::directSearchFeedback feedback_;
47  asr_direct_search_manager::directSearchResult result_;
49 
50  dynamic_reconfigure::Server<asr_direct_search_manager::DynamicParametersConfig> mDynamicReconfigServer;
51 
52  void initSearchMode() {
53  int directSearchMode;
54  nh_.getParam("directSearchMode", directSearchMode);
55  ROS_INFO_STREAM("Param: directSearchMode: " << directSearchMode);
56 
57  if (directSearchMode == 1) {
58  ROS_INFO_STREAM("direct search runs with grid_manager");
59  std::string initializedGridFilePath;
60  nh_.getParam("initializedGridFilePath", initializedGridFilePath);
61  ROS_INFO_STREAM("Initialized grid file path: " << initializedGridFilePath);
62  directSearchHandlerPtr = DirectSearchHandlerPtr(new GridManager(initializedGridFilePath));
63  } else if (directSearchMode == 2) {
64  ROS_INFO_STREAM("direct search runs with recording_manager");
65  std::string recordFilePath;
66  nh_.getParam("recordFilePath", recordFilePath);
67  ROS_INFO_STREAM("Record file path: " << recordFilePath);
68  directSearchHandlerPtr = DirectSearchHandlerPtr(new RecordingManager(recordFilePath));
69  } else {
70  ROS_INFO_STREAM("direct search runs with grid_initialisation");
71  std::string gridFilePath;
72  nh_.getParam("gridFilePath", gridFilePath);
73  ROS_INFO_STREAM("Grid file path: " << gridFilePath);
74  directSearchHandlerPtr = DirectSearchHandlerPtr(new GridInitialisation(gridFilePath));
75  }
76 
77  directSearchHandlerPtr->initHandler();
78  ROS_INFO_STREAM("Initialization finished");
79  }
80 
81 
82  void dynamicReconfigureCallback(asr_direct_search_manager::DynamicParametersConfig &config, uint32_t level) {
83  ROS_INFO_STREAM("Dynamic reconfigure called with level: " << level);
84  boost::lock_guard<boost::mutex> guard(mtx_);
85 
86  nh_.setParam("fovH", config.fovH);
87  nh_.setParam("fovV", config.fovV);
88  nh_.setParam("clearVision", config.clearVision);
89 
90  nh_.setParam("directSearchMode", config.directSearchMode);
91  nh_.setParam("distanceFunc", config.distanceFunc);
92 
93  nh_.setParam("reorderPosesByNBV", config.reorderPosesByNBV);
94  nh_.setParam("reorderPosesByTSP", config.reorderPosesByTSP);
95 
96  nh_.setParam("viewCenterPositionDistanceThreshold", config.viewCenterPositionDistanceThreshold);
97 
98  nh_.setParam("filterMinimumNumberOfDeletedNormals", config.filterMinimumNumberOfDeletedNormals);
99  nh_.setParam("filterIsPositionAllowed", config.filterIsPositionAllowed);
100  nh_.setParam("concatApproxEqualsPoses", config.concatApproxEqualsPoses);
101 
102  nh_.setParam("concatRobotPosePositionDistanceThreshold", config.concatRobotPosePositionDistanceThreshold);
103  nh_.setParam("concatRobotPoseOrientationRadDistanceThreshold", config.concatRobotPoseOrientationRadDistanceThreshold);
104 
105  nh_.setParam("gridFilePath", config.gridFilePath);
106  nh_.setParam("initializedGridFilePath", config.initializedGridFilePath);
107  nh_.setParam("recordFilePath", config.recordFilePath);
108 
111  initSearchMode();
112  }
113 
114 public:
115 
117  nh_(ros::this_node::getName()), as_(nh_, ros::this_node::getName(), boost::bind(&directSearchAction::executeCB, this, _1), false),
118  feedback_(), result_(), directSearchHandlerPtr(), mDynamicReconfigServer(nh_) {
119 
120  ROS_INFO_STREAM("Begin of initialization");
121 
122  dynamic_reconfigure::Server<asr_direct_search_manager::DynamicParametersConfig>::CallbackType f = boost::bind(&directSearchAction::dynamicReconfigureCallback, this, _1, _2);
123  // setCallback calls the callback at first start
124  mDynamicReconfigServer.setCallback(f);
125 
126  as_.start();
127  }
128 
129  bool setResults() {
130  result_.remainingPTUPoses = directSearchHandlerPtr->getRemainingPTUPoses();
131  result_.remainingRobotPoses = directSearchHandlerPtr->getRemainingRobotPoses();
132  result_.remainingPosesDistances = directSearchHandlerPtr->getRemainingPosesDistances();
133  result_.isNoPoseLeft = directSearchHandlerPtr->getIsNoPoseLeft();
134  result_.isSameRobotPoseAsBefore = directSearchHandlerPtr->getIsSameRobotPoseAsBefore();
135  result_.arePosesFromDemonstrationLeft = directSearchHandlerPtr->getArePosesFromDemonstrationLeft();
136 
137  result_.filteredSearchedObjectTypesAndIds = directSearchHandlerPtr->getActualFilteredSearchedObjectTypesAndIds();
138 
139  PosePtr actualRobotPose = directSearchHandlerPtr->getActualRobotPosePtr();
140  if (actualRobotPose) {
141  result_.goalRobotPose = *actualRobotPose;
142  } else {
143  result_.goalRobotPose = geometry_msgs::Pose();
144  }
145 
146  PosePtr actualCameraPose = directSearchHandlerPtr->getActualCameraPosePtr();
147  if (actualCameraPose) {
148  result_.goalCameraPose = *actualCameraPose;
149  } else {
150  result_.goalCameraPose = geometry_msgs::Pose();
151  }
152 
153  PtuTuplePtr actualPTU = directSearchHandlerPtr->getActualPtuPtr();
154  if (actualPTU) {
155  result_.pan = actualPTU->getPan();
156  result_.tilt = actualPTU->getTilt();
157  } else {
158  result_.pan = 0;
159  result_.tilt = 0;
160  }
161 
162  ROS_DEBUG_STREAM("result:\n" << result_);
163  return true;
164  }
165 
166  void executeCB(const asr_direct_search_manager::directSearchGoalConstPtr &goal) {
167  ROS_INFO_STREAM("Received command: " << goal->command);
168  boost::lock_guard<boost::mutex> guard(mtx_);
169 
170  const std::string &command = goal->command;
171  bool success = true;
172  if (command == "Reset") {
173  success &= directSearchHandlerPtr->resetHandler();
174  } else if(command == "BackToInitial") {
175  success &= directSearchHandlerPtr->backToInitial(goal->searchedObjectTypesAndIds);
176  } else if(command == "GetGoalCameraPose") {
177  success &= directSearchHandlerPtr->getNextRobotState(goal->searchedObjectTypesAndIds);
178  } else {
179  ROS_WARN("Unknown command!");
180  return;
181  }
182 
183  success &= setResults();
184  if (success) {
185  as_.setSucceeded(result_);
186  } else {
187  as_.setAborted(result_);
188  }
189 
190  }
191 
192 };
193 
194 }
195 
196 int main(int argc, char** argv) {
197  ros::init(argc, argv, "asr_direct_search_manager");
198  ros::start();
199 
200  directSearchWS::directSearchAction asr_direct_search_manager;
201  ros::spin();
202 
203  return 0;
204 }
205 
206 
207 
ROSCPP_DECL void start()
f
int main(int argc, char **argv)
Definition: main.cpp:196
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::string getName(void *handle)
actionlib::SimpleActionServer< asr_direct_search_manager::directSearchAction > as_
Definition: main.cpp:44
#define ROS_WARN(...)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
DirectSearchHandlerPtr directSearchHandlerPtr
Definition: main.cpp:48
ROSCPP_DECL void spin(Spinner &spinner)
asr_direct_search_manager::directSearchResult result_
Definition: main.cpp:47
ROSLIB_DECL std::string command(const std::string &cmd)
asr_direct_search_manager::directSearchFeedback feedback_
Definition: main.cpp:46
#define ROS_DEBUG_STREAM(args)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
#define ROS_INFO_STREAM(args)
dynamic_reconfigure::Server< asr_direct_search_manager::DynamicParametersConfig > mDynamicReconfigServer
Definition: main.cpp:50
boost::shared_ptr< DirectSearchHandler > DirectSearchHandlerPtr
bool getParam(const std::string &key, std::string &s) const
void executeCB(const asr_direct_search_manager::directSearchGoalConstPtr &goal)
Definition: main.cpp:166
goal
Definition: test.py:46
void dynamicReconfigureCallback(asr_direct_search_manager::DynamicParametersConfig &config, uint32_t level)
Definition: main.cpp:82
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const


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