NavigationState.cpp
Go to the documentation of this file.
1 #include <rsm_additions/NavigationState.h>
2 
3 namespace rsm {
4 
5 NavigationState::NavigationState() {
6 }
7 
8 NavigationState::~NavigationState() {
9 }
10 
11 void NavigationState::onSetup() {
12  //initialize services, publisher and subscriber
13  ros::NodeHandle private_nh("~");
14  private_nh.param("pose_tolerance", _pose_tolerance, 0.1);
15  private_nh.param("idle_timer_duration", _idle_timer_duration, 10.0);
16  private_nh.param("unstuck_timer_duration", _unstuck_timer_duration, 5.0);
17  private_nh.param("idle_timer_behavior", _idle_timer_behavior, false);
18 
19  if (_idle_timer_duration < _unstuck_timer_duration) {
20  ROS_WARN(
21  "Idle timer duration must be longer than unstuck timer duration, setting the former to the latter + 1s!");
22  _idle_timer_duration = _unstuck_timer_duration + 1.0;
23  }
24 
25  ros::NodeHandle nh("rsm");
26  _get_navigation_goal_service =
27  nh.serviceClient<rsm_msgs::GetNavigationGoal>("getNavigationGoal");
28  _navigation_goal_completed_service = nh.serviceClient<
29  rsm_msgs::GoalCompleted>("navigationGoalCompleted");
30  _get_robot_pose_service = nh.serviceClient<rsm_msgs::GetRobotPose>(
31  "getRobotPose");
32  _get_reverse_mode_service = nh.serviceClient<std_srvs::Trigger>(
33  "getReverseMode");
34  _reverse_mode_subscriber = nh.subscribe<std_msgs::Bool>("reverseMode", 10,
35  &NavigationState::reverseModeCallback, this);
36  _get_exploration_mode_service = nh.serviceClient<std_srvs::Trigger>(
37  "getExplorationMode");
38  _operation_mode_subscriber = nh.subscribe<rsm_msgs::OperationMode>(
39  "operationMode", 1, &NavigationState::operationModeCallback, this);
40  _idle_timer = _nh.createTimer(ros::Duration(_idle_timer_duration),
41  &NavigationState::idleTimerCallback, this, false, false);
42  _unstuck_timer = _nh.createTimer(ros::Duration(_unstuck_timer_duration),
43  &NavigationState::unstuckTimerCallback, this, false, false);
44  //initialize variables
45  _name = "Navigation";
46  _navigation_mode = -1;
47  _goal_active = false;
48  _reverse_mode = false;
49  _exploration_mode = -1;
50  _robot_did_move = false;
51  _operation_mode = rsm_msgs::OperationMode::STOPPED;
52  _navigation_completed_status = rsm_msgs::GoalStatus::ACTIVE;
53  _unstucking_executed = false;
54  _unstucking_robot = false;
55 }
56 
57 void NavigationState::onEntry() {
58  //Request navigation goal from Service Provider
59  rsm_msgs::GetNavigationGoal srv;
60  if (_get_navigation_goal_service.call(srv)) {
61  _nav_goal = srv.response.goal;
62  _navigation_mode = srv.response.navigationMode;
63  _routine = srv.response.routine;
64  switch (_navigation_mode) {
65  case EXPLORATION:
66  _name = "E: Navigation";
67  break;
68  case WAYPOINT_FOLLOWING:
69  _name = "W: Navigation";
70  break;
71  case SIMPLE_GOAL:
72  _name = "G: Navigation";
73  break;
74  default:
75  _name = "Navigation";
76  break;
77  }
78  } else {
79  ROS_ERROR("Failed to call Get Navigation Goal service");
80  abortNavigation();
81  }
82  //Check if reverse mode is active with Service Provider
83  std_srvs::Trigger srv2;
84  if (_get_reverse_mode_service.call(srv2)) {
85  _reverse_mode = srv2.response.success;
86  if (_reverse_mode) {
87  _move_base_client.reset(
88  new MoveBaseClient("move_base_reverse", true));
89  } else {
90  _move_base_client.reset(new MoveBaseClient("move_base", true));
91  }
92  } else {
93  ROS_ERROR("Failed to call Get Reverse Mode service");
94  _move_base_client.reset(new MoveBaseClient("move_base", true));
95  }
96  //Get exploration mode from Service Provider if exploration is active
97  if (_navigation_mode == EXPLORATION) {
98  std_srvs::Trigger srv2;
99  if (_get_exploration_mode_service.call(srv2)) {
100  _exploration_mode = srv2.response.success;
101  if (_exploration_mode) {
102  ros::NodeHandle nh("rsm");
103  _get_goal_obsolete_subscriber = nh.subscribe("goalObsolete", 1,
104  &NavigationState::goalObsoleteCallback, this);
105  }
106  } else {
107  ROS_ERROR("Failed to call Get Exploration Mode service");
108  abortNavigation();
109  }
110  }
111 }
112 
113 void NavigationState::onActive() {
114  if (_move_base_client->isServerConnected()) {
115  if (_goal_active) {
116  if (_move_base_client->getState().isDone()) {
117  if (_move_base_client->getState().state_
118  == actionlib::SimpleClientGoalState::SUCCEEDED) {
119  if (!_interrupt_occured) {
120  switch (_navigation_mode) {
121  case EXPLORATION: {
122  _stateinterface->transitionToVolatileState(
123  _stateinterface->getPluginState(
124  MAPPING_STATE));
125  break;
126  }
127  case WAYPOINT_FOLLOWING: {
128  if (_routine.empty()) {
129  _navigation_completed_status =
130  rsm_msgs::GoalStatus::REACHED;
131  _stateinterface->transitionToVolatileState(
132  boost::make_shared<
133  WaypointFollowingState>());
134  } else {
135  _stateinterface->transitionToVolatileState(
136  _stateinterface->getPluginState(
137  ROUTINE_STATE, _routine));
138  }
139  break;
140  }
141  default: {
142  abortNavigation();
143  break;
144  }
145  }
146  }
147  } else {
148  if (!_interrupt_occured) {
149  if (!_unstucking_robot) {
150  //try reverse navigation
151  ROS_WARN_STREAM(
152  "Try to unstuck robot by using reversed move base");
153  if (_goal_active) {
154  _move_base_client->cancelGoal();
155  }
156  _goal_active = false;
157  if (_reverse_mode) {
158  _move_base_client.reset(
159  new MoveBaseClient("move_base", true));
160  } else {
161  _move_base_client.reset(
162  new MoveBaseClient("move_base_reverse",
163  true));
164  }
165  _unstuck_timer.start();
166  _unstucking_robot = true;
167  } else {
168  //navigation failed
169  _navigation_completed_status =
170  rsm_msgs::GoalStatus::FAILED;
171  switch (_navigation_mode) {
172  case EXPLORATION: {
173  _stateinterface->transitionToVolatileState(
174  _stateinterface->getPluginState(
175  CALCULATEGOAL_STATE));
176  break;
177  }
178  case WAYPOINT_FOLLOWING: {
179  _stateinterface->transitionToVolatileState(
180  boost::make_shared<
181  WaypointFollowingState>());
182  break;
183  }
184  default: {
185  abortNavigation();
186  break;
187  }
188  }
189  }
190  }
191  }
192  }
193  } else { //Initialize goal
194  move_base_msgs::MoveBaseGoal goal;
195  goal.target_pose.header.frame_id = "map";
196  goal.target_pose.header.stamp = ros::Time::now();
197  goal.target_pose.pose = _nav_goal;
198  _move_base_client->sendGoal(goal);
199  _goal_active = true;
200  rsm_msgs::GetRobotPose srv;
201  if (_get_robot_pose_service.call(srv)) {
202  tf::Pose current_pose;
203  tf::poseMsgToTF(srv.response.pose, current_pose);
204  tf::Matrix3x3 m(current_pose.getRotation());
205  double roll, pitch, yaw;
206  m.getRPY(roll, pitch, yaw);
207  _last_position = current_pose.getOrigin();
208  _last_yaw = yaw;
209  }
210  //Start timer for checking navigation being stuck for too long without proper message from Move Base
211  _idle_timer.start();
212  }
213  }
214 }
215 
216 void NavigationState::onExit() {
217  if (_goal_active) {
218  _move_base_client->cancelGoal();
219  }
220  if (_navigation_completed_status != rsm_msgs::GoalStatus::ACTIVE) {
221  rsm_msgs::GoalCompleted srv;
222  srv.request.status.goal_status = _navigation_completed_status;
223  srv.request.navigation_status = true;
224  if (!_navigation_goal_completed_service.call(srv)) {
225  ROS_ERROR("Failed to call Complete Navigation Goal service");
226  }
227  }
228 }
229 
230 void NavigationState::onExplorationStart(bool &success, std::string &message) {
231  switch (_navigation_mode) {
232  case EXPLORATION:
233  success = false;
234  message = "Exploration running";
235  break;
236  case WAYPOINT_FOLLOWING:
237  success = false;
238  message = "Waypoint following running";
239  break;
240  case SIMPLE_GOAL:
241  success = false;
242  message = "Simple Goal running";
243  break;
244  default:
245  success = false;
246  message = "Nothing running";
247  break;
248  }
249 }
250 
251 void NavigationState::onExplorationStop(bool &success, std::string &message) {
252  switch (_navigation_mode) {
253  case EXPLORATION:
254  success = true;
255  message = "Exploration stopped";
256  abortNavigation();
257  break;
258  case WAYPOINT_FOLLOWING:
259  success = false;
260  message = "Waypoint following running";
261  break;
262  case SIMPLE_GOAL:
263  success = false;
264  message = "Simple Goal running";
265  break;
266  default:
267  success = false;
268  message = "Nothing running";
269  break;
270  }
271 }
272 
273 void NavigationState::onWaypointFollowingStart(bool &success,
274  std::string &message) {
275  success = false;
276  switch (_navigation_mode) {
277  case EXPLORATION:
278  message = "Exploration running";
279  break;
280  case WAYPOINT_FOLLOWING:
281  message = "Waypoint following running";
282  break;
283  case SIMPLE_GOAL:
284  message = "Simple Goal running";
285  break;
286  default:
287  message = "Nothing running";
288  break;
289  }
290 }
291 
292 void NavigationState::onWaypointFollowingStop(bool &success,
293  std::string &message) {
294  switch (_navigation_mode) {
295  case EXPLORATION:
296  success = false;
297  message = "Exploration running";
298  break;
299  case WAYPOINT_FOLLOWING:
300  success = true;
301  message = "Waypoint following stopped";
302  abortNavigation();
303  break;
304  case SIMPLE_GOAL:
305  success = false;
306  message = "Simple Goal running";
307  break;
308  default:
309  success = false;
310  message = "Nothing running";
311  break;
312  }
313 }
314 
315 void NavigationState::onInterrupt(int interrupt) {
316  _navigation_completed_status = rsm_msgs::GoalStatus::ABORTED;
317  switch (interrupt) {
318  case EMERGENCY_STOP_INTERRUPT:
319  _stateinterface->transitionToVolatileState(
320  boost::make_shared<EmergencyStopState>());
321  _interrupt_occured = true;
322  break;
323  case TELEOPERATION_INTERRUPT:
324  _stateinterface->transitionToVolatileState(
325  boost::make_shared<TeleoperationState>());
326  _interrupt_occured = true;
327  break;
328  case SIMPLE_GOAL_INTERRUPT:
329  _stateinterface->transitionToVolatileState(
330  _stateinterface->getPluginState(NAVIGATION_STATE));
331  _interrupt_occured = true;
332  break;
333  case SIMPLE_GOAL_STOP_INTERRUPT:
334  if (_navigation_mode == SIMPLE_GOAL) {
335  _stateinterface->transitionToVolatileState(
336  boost::make_shared<IdleState>());
337  }
338  break;
339  }
340 }
341 
342 void NavigationState::idleTimerCallback(const ros::TimerEvent &event) {
343  if (comparePose()) { //restart timer if robot did move
344  _idle_timer.stop();
345  _idle_timer.start();
346  return;
347  }
348  if (!_unstucking_executed && !_unstucking_robot) {
349  //try reverse navigation
350  ROS_WARN("Try to unstuck robot by using reversed move base");
351  if (_goal_active) {
352  _move_base_client->cancelGoal();
353  }
354  _goal_active = false;
355  if (_reverse_mode) {
356  _move_base_client.reset(new MoveBaseClient("move_base", true));
357  } else {
358  _move_base_client.reset(
359  new MoveBaseClient("move_base_reverse", true));
360  }
361  _unstuck_timer.start();
362  _unstucking_robot = true;
363  _idle_timer.start();
364  } else {
365  ROS_WARN("Navigation aborted because robot appears to be stuck");
366  if (_idle_timer_behavior) { //end navigation
367  abortNavigation();
368  } else { //declare goal as failed
369  _navigation_completed_status = rsm_msgs::GoalStatus::FAILED;
370  switch (_navigation_mode) {
371  case EXPLORATION: {
372  _stateinterface->transitionToVolatileState(
373  _stateinterface->getPluginState(
374  CALCULATEGOAL_STATE));
375  break;
376  }
377  case WAYPOINT_FOLLOWING: {
378  _stateinterface->transitionToVolatileState(
379  boost::make_shared<WaypointFollowingState>());
380  break;
381  }
382  default: {
383  abortNavigation();
384  break;
385  }
386  }
387  }
388  }
389 }
390 
391 void NavigationState::unstuckTimerCallback(const ros::TimerEvent &event) {
392  if (_goal_active) {
393  _move_base_client->cancelGoal();
394  }
395  _goal_active = false;
396  if (_reverse_mode) {
397  _move_base_client.reset(new MoveBaseClient("move_base_reverse", true));
398  } else {
399  _move_base_client.reset(new MoveBaseClient("move_base", true));
400  }
401  _unstucking_robot = false;
402  _unstucking_executed = true;
403 }
404 
405 bool NavigationState::comparePose() {
406  if (_operation_mode == rsm_msgs::OperationMode::AUTONOMOUS) {
407  tf::Pose current_pose;
408  rsm_msgs::GetRobotPose srv;
409  if (_get_robot_pose_service.call(srv)) {
410  tf::poseMsgToTF(srv.response.pose, current_pose);
411  tf::Matrix3x3 m(current_pose.getRotation());
412  double roll, pitch, yaw;
413  m.getRPY(roll, pitch, yaw);
414  if (std::abs(current_pose.getOrigin().y() - _last_position.y())
415  > _pose_tolerance
416  || std::abs(
417  current_pose.getOrigin().y() - _last_position.y())
418  > _pose_tolerance
419  || std::abs(yaw - _last_yaw) > _pose_tolerance) {
420  _robot_did_move = true;
421  _last_position = current_pose.getOrigin();
422  _last_yaw = yaw;
423  return true;
424  } else {
425  return false;
426  }
427  } else {
428  ROS_ERROR("Failed to call Get Robot Pose service");
429  }
430  }
431  return true;
432 }
433 
434 void NavigationState::goalObsoleteCallback(
435  const std_msgs::Bool::ConstPtr &msg) {
436  if (msg->data && !_interrupt_occured) {
437  comparePose(); //check if robot moved
438  _navigation_completed_status = rsm_msgs::GoalStatus::ABORTED;
439  _stateinterface->transitionToVolatileState(
440  _stateinterface->getPluginState(
441  _robot_did_move ? MAPPING_STATE : CALCULATEGOAL_STATE));
442  }
443 }
444 
445 void NavigationState::reverseModeCallback(
446  const std_msgs::Bool::ConstPtr &reverse_mode) {
447  if (_reverse_mode != reverse_mode->data) {
448  if (_goal_active) {
449  _move_base_client->cancelGoal();
450  }
451  _goal_active = false;
452  _reverse_mode = reverse_mode->data;
453  if (_reverse_mode) {
454  _move_base_client.reset(
455  new MoveBaseClient("move_base_reverse", true));
456  } else {
457  _move_base_client.reset(new MoveBaseClient("move_base", true));
458  }
459  }
460 }
461 
462 void NavigationState::operationModeCallback(
463  const rsm_msgs::OperationMode::ConstPtr &operation_mode) {
464  _operation_mode = operation_mode->mode;
465 }
466 
467 void NavigationState::abortNavigation() {
468  if (!_interrupt_occured) {
469  _navigation_completed_status = rsm_msgs::GoalStatus::ABORTED;
470  _stateinterface->transitionToVolatileState(
471  boost::make_shared<IdleState>());
472  }
473 }
474 
475 }
476 
477 PLUGINLIB_EXPORT_CLASS(rsm::NavigationState, rsm::BaseState)
rsm::NavigationState::onWaypointFollowingStart
void onWaypointFollowingStart(bool &success, std::string &message)
Definition: NavigationState.cpp:273
ros::NodeHandle::serviceClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
tf::poseMsgToTF
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
rsm::NavigationState::goalObsoleteCallback
void goalObsoleteCallback(const std_msgs::Bool::ConstPtr &msg)
Definition: NavigationState.cpp:434
rsm::NavigationState::_get_goal_obsolete_subscriber
ros::Subscriber _get_goal_obsolete_subscriber
Definition: NavigationState.h:99
ros::NodeHandle
rsm::NavigationState::onExplorationStart
void onExplorationStart(bool &success, std::string &message)
Definition: NavigationState.cpp:230
rsm::NavigationState::_operation_mode_subscriber
ros::Subscriber _operation_mode_subscriber
Definition: NavigationState.h:101
rsm::StateInterface::getPluginState
boost::shared_ptr< rsm::BaseState > getPluginState(int plugin_type, std::string routine="")
tf::Transform::getRotation
Quaternion getRotation() const
MAPPING_STATE
#define MAPPING_STATE
rsm::NavigationState::_get_reverse_mode_service
ros::ServiceClient _get_reverse_mode_service
Definition: NavigationState.h:98
ros::NodeHandle::createTimer
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
rsm::NavigationState::_get_navigation_goal_service
ros::ServiceClient _get_navigation_goal_service
Definition: NavigationState.h:94
NAVIGATION_STATE
#define NAVIGATION_STATE
SIMPLE_GOAL_STOP_INTERRUPT
#define SIMPLE_GOAL_STOP_INTERRUPT
rsm::NavigationState
State being active when the robot should move to a previously set goal. First obtains the goal and th...
Definition: NavigationState.h:28
ros::Timer::stop
void stop()
ros::ServiceClient::call
bool call(MReq &req, MRes &res)
ros::Timer::start
void start()
ROUTINE_STATE
#define ROUTINE_STATE
rsm::NavigationState::onWaypointFollowingStop
void onWaypointFollowingStop(bool &success, std::string &message)
Definition: NavigationState.cpp:292
rsm::NavigationState::_nav_goal
geometry_msgs::Pose _nav_goal
Definition: NavigationState.h:108
ROS_WARN
#define ROS_WARN(...)
rsm::StateInterface::transitionToVolatileState
void transitionToVolatileState(boost::shared_ptr< rsm::BaseState > next_state)
rsm::BaseState::_interrupt_occured
bool _interrupt_occured
WAYPOINT_FOLLOWING
#define WAYPOINT_FOLLOWING
TELEOPERATION_INTERRUPT
#define TELEOPERATION_INTERRUPT
rsm::NavigationState::onInterrupt
void onInterrupt(int interrupt)
Called when an operation mode interrupt was received.
Definition: NavigationState.cpp:315
rsm::NavigationState::unstuckTimerCallback
void unstuckTimerCallback(const ros::TimerEvent &event)
Callback for switched mode navigation to unstuck robot.
Definition: NavigationState.cpp:391
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
rsm::NavigationState::_last_position
tf::Vector3 _last_position
Definition: NavigationState.h:124
rsm::NavigationState::_get_robot_pose_service
ros::ServiceClient _get_robot_pose_service
Definition: NavigationState.h:96
CALCULATEGOAL_STATE
#define CALCULATEGOAL_STATE
rsm::BaseState::_name
std::string _name
rsm::NavigationState::idleTimerCallback
void idleTimerCallback(const ros::TimerEvent &event)
Callback for idle timer.
Definition: NavigationState.cpp:342
rsm::BaseState::_stateinterface
StateInterface * _stateinterface
EMERGENCY_STOP_INTERRUPT
#define EMERGENCY_STOP_INTERRUPT
EXPLORATION
#define EXPLORATION
rsm::NavigationState::onExplorationStop
void onExplorationStop(bool &success, std::string &message)
Definition: NavigationState.cpp:251
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
rsm::NavigationState::_get_exploration_mode_service
ros::ServiceClient _get_exploration_mode_service
Definition: NavigationState.h:97
rsm::NavigationState::operationModeCallback
void operationModeCallback(const rsm_msgs::OperationMode::ConstPtr &operation_mode)
Definition: NavigationState.cpp:462
rsm::NavigationState::_navigation_goal_completed_service
ros::ServiceClient _navigation_goal_completed_service
Definition: NavigationState.h:95
rsm::NavigationState::_nh
ros::NodeHandle _nh
Definition: NavigationState.h:93
rsm::NavigationState::MoveBaseClient
actionlib::SimpleActionClient< move_base_msgs::MoveBaseAction > MoveBaseClient
Definition: NavigationState.h:90
tf::Matrix3x3::getRPY
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
ros::Time::now
static Time now()
rsm::NavigationState::reverseModeCallback
void reverseModeCallback(const std_msgs::Bool::ConstPtr &reverse_mode)
Definition: NavigationState.cpp:445
SIMPLE_GOAL_INTERRUPT
#define SIMPLE_GOAL_INTERRUPT
SIMPLE_GOAL
#define SIMPLE_GOAL
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
rsm::NavigationState::_move_base_client
boost::shared_ptr< MoveBaseClient > _move_base_client
Definition: NavigationState.h:91
ROS_ERROR
#define ROS_ERROR(...)
rsm::NavigationState::_reverse_mode_subscriber
ros::Subscriber _reverse_mode_subscriber
Definition: NavigationState.h:100

rsm_additions
Author(s): Marco Steinbrink
autogenerated on Mon Feb 28 2022 23:28:21