ReversingRoutineState.cpp
Go to the documentation of this file.
2 
3 namespace rsm {
4 
6 }
7 
9 }
10 
12 
13  //initialize services, publisher and subscriber
14  ros::NodeHandle nh("rsm");
15  _set_reverse_moving_service = nh.serviceClient<std_srvs::SetBool>(
16  "setReverseMode");
17  _get_reverse_moving_service = nh.serviceClient<std_srvs::Trigger>(
18  "getReverseMode");
20  rsm_msgs::GoalCompleted>("navigationGoalCompleted");
21  //initialize variables
22  _name = "W: Reversing";
23  _reverse_mode_active = false;
24  _navigation_completed_status = rsm_msgs::GoalStatus::ABORTED;
25 }
26 
28  //Request current reverse mode status from Service Provider
29  std_srvs::Trigger srv;
31  _reverse_mode_active = srv.response.success;
32  } else {
33  ROS_ERROR("Failed to call Get Reverse Mode service");
34  if (!_interrupt_occured) {
35  _navigation_completed_status = rsm_msgs::GoalStatus::FAILED;
37  boost::make_shared<WaypointFollowingState>());
38  }
39  }
40 }
41 
43  //Toggle reverse mode
44  std_srvs::SetBool srv;
45  srv.request.data = !_reverse_mode_active;
47  ROS_ERROR("Failed to call Set Reverse Mode service");
48  }
49  if (!_interrupt_occured) {
50  _navigation_completed_status = rsm_msgs::GoalStatus::REACHED;
52  boost::make_shared<WaypointFollowingState>());
53  }
54 }
55 
57  rsm_msgs::GoalCompleted srv;
58  srv.request.status.goal_status = _navigation_completed_status;
60  ROS_ERROR("Failed to call Complete Navigation Goal service");
61  }
62 }
63 
65  std::string &message) {
66  success = false;
67  message = "Waypoint following running";
68 }
69 
71  std::string &message) {
72  success = false;
73  message = "Waypoint following running";
74 }
75 
77  std::string &message) {
78  success = false;
79  message = "Waypoint following running";
80 }
81 
83  std::string &message) {
84  success = true;
85  message = "Waypoint following stopped";
86  _stateinterface->transitionToVolatileState(boost::make_shared<IdleState>());
87 }
88 
90  switch (interrupt) {
93  boost::make_shared<EmergencyStopState>());
94  _interrupt_occured = true;
95  break;
98  boost::make_shared<TeleoperationState>());
99  _interrupt_occured = true;
100  break;
104  _interrupt_occured = true;
105  break;
106  }
107 }
108 
109 }
110 
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ros::ServiceClient _set_reverse_moving_service
boost::shared_ptr< rsm::BaseState > getPluginState(int plugin_type, std::string routine="")
#define NAVIGATION_STATE
void onInterrupt(int interrupt)
Called when an operation mode interrupt was received.
ros::ServiceClient _navigation_goal_completed_service
bool call(MReq &req, MRes &res)
void onWaypointFollowingStart(bool &success, std::string &message)
void transitionToVolatileState(boost::shared_ptr< rsm::BaseState > next_state)
bool _interrupt_occured
#define TELEOPERATION_INTERRUPT
ros::ServiceClient _get_reverse_moving_service
void onExplorationStart(bool &success, std::string &message)
void onExplorationStop(bool &success, std::string &message)
std::string _name
StateInterface * _stateinterface
void onWaypointFollowingStop(bool &success, std::string &message)
#define EMERGENCY_STOP_INTERRUPT
#define SIMPLE_GOAL_INTERRUPT
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
Routine state that toggles the reverse mode.


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