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 nh("rsm");
14  _get_navigation_goal_service =
15  nh.serviceClient<rsm_msgs::GetNavigationGoal>("getNavigationGoal");
16  _navigation_goal_completed_service = nh.serviceClient<
17  rsm_msgs::GoalCompleted>("navigationGoalCompleted");
18  _get_robot_pose_service = nh.serviceClient<rsm_msgs::GetRobotPose>(
19  "getRobotPose");
20  _get_reverse_mode_service = nh.serviceClient<std_srvs::Trigger>(
21  "getReverseMode");
22  _reverse_mode_subscriber = nh.subscribe<std_msgs::Bool>("reverseMode", 10,
23  &NavigationState::reverseModeCallback, this);
24  _get_exploration_mode_service = nh.serviceClient<std_srvs::Trigger>(
25  "getExplorationMode");
26  _idle_timer = _nh.createTimer(ros::Duration(15.0),
27  &NavigationState::timerCallback, this, true, false);
28  //initialize variables
29  _name = "Navigation";
30  _navigation_mode = -1;
31  _goal_active = false;
32  _reverse_mode = false;
33  _exploration_mode = -1;
34  _operation_mode = rsm_msgs::OperationMode::STOPPED;
35  _navigation_completed_status = rsm_msgs::GoalStatus::ACTIVE;
36 }
37 
38 void NavigationState::onEntry() {
39  //Request navigation goal from Service Provider
40  rsm_msgs::GetNavigationGoal srv;
41  if (_get_navigation_goal_service.call(srv)) {
42  _nav_goal = srv.response.goal;
43  _navigation_mode = srv.response.navigationMode;
44  _routine = srv.response.routine;
45  switch (_navigation_mode) {
46  case EXPLORATION:
47  _name = "E: Navigation";
48  break;
49  case WAYPOINT_FOLLOWING:
50  _name = "W: Navigation";
51  break;
52  case SIMPLE_GOAL:
53  _name = "G: Navigation";
54  break;
55  default:
56  _name = "Navigation";
57  break;
58  }
59  } else {
60  ROS_ERROR("Failed to call Get Navigation Goal service");
61  abortNavigation();
62  }
63  //Check if reverse mode is active with Service Provider
64  std_srvs::Trigger srv2;
65  if (_get_reverse_mode_service.call(srv2)) {
66  _reverse_mode = srv2.response.success;
67  if (_reverse_mode) {
68  _move_base_client.reset(
69  new MoveBaseClient("move_base_reverse", true));
70  } else {
71  _move_base_client.reset(new MoveBaseClient("move_base", true));
72  }
73  } else {
74  ROS_ERROR("Failed to call Get Reverse Mode service");
75  _move_base_client.reset(new MoveBaseClient("move_base", true));
76  }
77  //Get exploration mode from Service Provider if exploration is active
78  if (_navigation_mode == EXPLORATION) {
79  std_srvs::Trigger srv2;
80  if (_get_exploration_mode_service.call(srv2)) {
81  _exploration_mode = srv2.response.success;
82  if (_exploration_mode) {
83  ros::NodeHandle nh("rsm");
84  _get_goal_obsolete_subscriber = nh.subscribe("goalObsolete", 1,
85  &NavigationState::goalObsoleteCallback, this);
86  }
87  } else {
88  ROS_ERROR("Failed to call Get Exploration Mode service");
89  abortNavigation();
90  }
91  }
92  //Start timer for checking navigation being stuck for too long (15 secs) without proper message from Move Base
93  _idle_timer.start();
94 }
95 
96 void NavigationState::onActive() {
97  if (_move_base_client->isServerConnected()) {
98  if (_goal_active) {
99  if (_move_base_client->getState().isDone()) {
100  if (_move_base_client->getState().state_
101  == actionlib::SimpleClientGoalState::SUCCEEDED) {
102  if (!_interrupt_occured) {
103  switch (_navigation_mode) {
104  case EXPLORATION: {
105  _stateinterface->transitionToVolatileState(
106  _stateinterface->getPluginState(
107  MAPPING_STATE));
108  break;
109  }
110  case WAYPOINT_FOLLOWING: {
111  if (_routine.empty()) {
112  _navigation_completed_status =
113  rsm_msgs::GoalStatus::REACHED;
114  _stateinterface->transitionToVolatileState(
115  boost::make_shared<
116  WaypointFollowingState>());
117  } else {
118  _stateinterface->transitionToVolatileState(
119  _stateinterface->getPluginState(
120  ROUTINE_STATE, _routine));
121  }
122  break;
123  }
124  default: {
125  abortNavigation();
126  break;
127  }
128  }
129  }
130  } else {
131  if (!_interrupt_occured) {
132  _navigation_completed_status =
133  rsm_msgs::GoalStatus::FAILED;
134  switch (_navigation_mode) {
135  case EXPLORATION: {
136  _stateinterface->transitionToVolatileState(
137  _stateinterface->getPluginState(
138  CALCULATEGOAL_STATE));
139  break;
140  }
141  case WAYPOINT_FOLLOWING: {
142  _stateinterface->transitionToVolatileState(
143  boost::make_shared<WaypointFollowingState>());
144  break;
145  }
146  default: {
147  abortNavigation();
148  break;
149  }
150  }
151  }
152  }
153  } else {
154  //Check if robot moved for navigation failure check
155  comparePose();
156  }
157  } else {
158  //Initialize goal
159  move_base_msgs::MoveBaseGoal goal;
160  goal.target_pose.header.frame_id = "map";
161  goal.target_pose.header.stamp = ros::Time::now();
162  goal.target_pose.pose = _nav_goal;
163  _move_base_client->sendGoal(goal);
164  _goal_active = true;
165  }
166  }
167 }
168 
169 void NavigationState::onExit() {
170  if (_goal_active) {
171  _move_base_client->cancelGoal();
172  }
173  if (_navigation_completed_status != rsm_msgs::GoalStatus::ACTIVE) {
174  rsm_msgs::GoalCompleted srv;
175  srv.request.status.goal_status = _navigation_completed_status;
176  if (!_navigation_goal_completed_service.call(srv)) {
177  ROS_ERROR("Failed to call Complete Navigation Goal service");
178  }
179  }
180 }
181 
182 void NavigationState::onExplorationStart(bool &success, std::string &message) {
183  switch (_navigation_mode) {
184  case EXPLORATION:
185  success = false;
186  message = "Exploration running";
187  break;
188  case WAYPOINT_FOLLOWING:
189  success = false;
190  message = "Waypoint following running";
191  break;
192  case SIMPLE_GOAL:
193  success = false;
194  message = "Simple Goal running";
195  break;
196  default:
197  success = false;
198  message = "Nothing running";
199  break;
200  }
201 }
202 
203 void NavigationState::onExplorationStop(bool &success, std::string &message) {
204  switch (_navigation_mode) {
205  case EXPLORATION:
206  success = true;
207  message = "Exploration stopped";
208  abortNavigation();
209  break;
210  case WAYPOINT_FOLLOWING:
211  success = false;
212  message = "Waypoint following running";
213  break;
214  case SIMPLE_GOAL:
215  success = false;
216  message = "Simple Goal running";
217  break;
218  default:
219  success = false;
220  message = "Nothing running";
221  break;
222  }
223 }
224 
225 void NavigationState::onWaypointFollowingStart(bool &success,
226  std::string &message) {
227  success = false;
228  switch (_navigation_mode) {
229  case EXPLORATION:
230  message = "Exploration running";
231  break;
232  case WAYPOINT_FOLLOWING:
233  message = "Waypoint following running";
234  break;
235  case SIMPLE_GOAL:
236  message = "Simple Goal running";
237  break;
238  default:
239  message = "Nothing running";
240  break;
241  }
242 }
243 
244 void NavigationState::onWaypointFollowingStop(bool &success,
245  std::string &message) {
246  switch (_navigation_mode) {
247  case EXPLORATION:
248  success = false;
249  message = "Exploration running";
250  break;
251  case WAYPOINT_FOLLOWING:
252  success = true;
253  message = "Waypoint following stopped";
254  abortNavigation();
255  break;
256  case SIMPLE_GOAL:
257  success = false;
258  message = "Simple Goal running";
259  break;
260  default:
261  success = false;
262  message = "Nothing running";
263  break;
264  }
265 }
266 
267 void NavigationState::onInterrupt(int interrupt) {
268  _navigation_completed_status = rsm_msgs::GoalStatus::ABORTED;
269  switch (interrupt) {
270  case EMERGENCY_STOP_INTERRUPT:
271  _stateinterface->transitionToVolatileState(
272  boost::make_shared<EmergencyStopState>());
273  _interrupt_occured = true;
274  break;
275  case TELEOPERATION_INTERRUPT:
276  _stateinterface->transitionToVolatileState(
277  boost::make_shared<TeleoperationState>());
278  _interrupt_occured = true;
279  break;
280  case SIMPLE_GOAL_INTERRUPT:
281  _stateinterface->transitionToVolatileState(
282  _stateinterface->getPluginState(NAVIGATION_STATE));
283  _interrupt_occured = true;
284  break;
285  case SIMPLE_GOAL_STOP_INTERRUPT:
286  if (_navigation_mode == SIMPLE_GOAL) {
287  _stateinterface->transitionToVolatileState(
288  boost::make_shared<IdleState>());
289  }
290  break;
291  }
292 }
293 
294 void NavigationState::timerCallback(const ros::TimerEvent& event) {
295  ROS_ERROR("Navigation aborted because robot appears to be stuck");
296  abortNavigation();
297 }
298 
299 void NavigationState::comparePose() {
300  if (_operation_mode == rsm_msgs::OperationMode::AUTONOMOUS) {
301  if (_comparison_counter++ >= 5) { //only compare poses every 5th call to reduce load
302  tf::Pose current_pose;
303  rsm_msgs::GetRobotPose srv;
304  if (_get_robot_pose_service.call(srv)) {
305  tf::poseMsgToTF(srv.response.pose, current_pose);
306  tf::Pose pose_difference = current_pose.inverseTimes(
307  _last_pose);
308  if (pose_difference.getOrigin().x() < POSE_TOLERANCE
309  && pose_difference.getOrigin().y() < POSE_TOLERANCE
310  && pose_difference.getOrigin().z() < POSE_TOLERANCE
311  && pose_difference.getRotation().x() == 0.0
312  && pose_difference.getRotation().y() == 0.0
313  && pose_difference.getRotation().z() == 0.0
314  && pose_difference.getRotation().w() == 1.0) {
315  _idle_timer.start();
316  } else {
317  _idle_timer.stop();
318  }
319  _last_pose = current_pose;
320  _comparison_counter = 0;
321  } else {
322  ROS_ERROR("Failed to call Get Robot Pose service");
323  }
324  }
325  } else {
326  _idle_timer.stop();
327  }
328 }
329 
330 void NavigationState::goalObsoleteCallback(
331  const std_msgs::Bool::ConstPtr& msg) {
332  if (msg->data && !_interrupt_occured) {
333  _navigation_completed_status = rsm_msgs::GoalStatus::ABORTED;
334  _stateinterface->transitionToVolatileState(
335  _stateinterface->getPluginState(
336  MAPPING_STATE));
337  }
338 }
339 
340 void NavigationState::reverseModeCallback(
341  const std_msgs::Bool::ConstPtr& reverse_mode) {
342  if (_reverse_mode != reverse_mode->data) {
343  if (_goal_active) {
344  _move_base_client->cancelGoal();
345  }
346  _goal_active = false;
347  _reverse_mode = reverse_mode->data;
348  if (_reverse_mode) {
349  _move_base_client.reset(
350  new MoveBaseClient("move_base_reverse", true));
351  } else {
352  _move_base_client.reset(new MoveBaseClient("move_base", true));
353  }
354  }
355 }
356 
357 void NavigationState::operationModeCallback(
358  const rsm_msgs::OperationMode::ConstPtr& operation_mode) {
359  _operation_mode = operation_mode->mode;
360 }
361 
362 void NavigationState::abortNavigation() {
363  if (!_interrupt_occured) {
364  _navigation_completed_status = rsm_msgs::GoalStatus::ABORTED;
365  _stateinterface->transitionToVolatileState(
366  boost::make_shared<IdleState>());
367  }
368 }
369 
370 }
371 
372 PLUGINLIB_EXPORT_CLASS(rsm::NavigationState, rsm::BaseState)
rsm::NavigationState::onWaypointFollowingStart
void onWaypointFollowingStart(bool &success, std::string &message)
Definition: NavigationState.cpp:225
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:330
rsm::NavigationState::_get_goal_obsolete_subscriber
ros::Subscriber _get_goal_obsolete_subscriber
Definition: NavigationState.h:101
ros::NodeHandle
rsm::NavigationState::onExplorationStart
void onExplorationStart(bool &success, std::string &message)
Definition: NavigationState.cpp:182
rsm::StateInterface::getPluginState
boost::shared_ptr< rsm::BaseState > getPluginState(int plugin_type, std::string routine="")
MAPPING_STATE
#define MAPPING_STATE
rsm::NavigationState::_get_reverse_mode_service
ros::ServiceClient _get_reverse_mode_service
Definition: NavigationState.h:100
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:96
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:30
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:244
rsm::NavigationState::_nav_goal
geometry_msgs::Pose _nav_goal
Definition: NavigationState.h:108
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
tf::Vector3::x
TFSIMD_FORCE_INLINE const tfScalar & x() const
rsm::NavigationState::onInterrupt
void onInterrupt(int interrupt)
Called when an operation mode interrupt was received.
Definition: NavigationState.cpp:267
tf::Vector3::z
TFSIMD_FORCE_INLINE const tfScalar & z() const
rsm::NavigationState::_get_robot_pose_service
ros::ServiceClient _get_robot_pose_service
Definition: NavigationState.h:98
CALCULATEGOAL_STATE
#define CALCULATEGOAL_STATE
rsm::BaseState::_name
std::string _name
tf::Vector3::y
TFSIMD_FORCE_INLINE const tfScalar & y() const
ros::NodeHandle::createTimer
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
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:203
rsm::NavigationState::timerCallback
void timerCallback(const ros::TimerEvent &event)
Callback for idle timer.
Definition: NavigationState.cpp:294
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
rsm::NavigationState::_get_exploration_mode_service
ros::ServiceClient _get_exploration_mode_service
Definition: NavigationState.h:99
rsm::NavigationState::operationModeCallback
void operationModeCallback(const rsm_msgs::OperationMode::ConstPtr &operation_mode)
Definition: NavigationState.cpp:357
rsm::NavigationState::_navigation_goal_completed_service
ros::ServiceClient _navigation_goal_completed_service
Definition: NavigationState.h:97
rsm::NavigationState::_nh
ros::NodeHandle _nh
Definition: NavigationState.h:95
rsm::NavigationState::MoveBaseClient
actionlib::SimpleActionClient< move_base_msgs::MoveBaseAction > MoveBaseClient
Definition: NavigationState.h:92
tf::Transform::getRotation
Quaternion getRotation() const
ros::Time::now
static Time now()
rsm::NavigationState::reverseModeCallback
void reverseModeCallback(const std_msgs::Bool::ConstPtr &reverse_mode)
Definition: NavigationState.cpp:340
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:93
ROS_ERROR
#define ROS_ERROR(...)
tf::Transform::inverseTimes
Transform inverseTimes(const Transform &t) const
rsm::NavigationState::_reverse_mode_subscriber
ros::Subscriber _reverse_mode_subscriber
Definition: NavigationState.h:102

rsm_additions
Author(s): Marco Steinbrink
autogenerated on Tue Mar 16 2021 02:44:35