stuck_detector.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #include <std_srvs/Trigger.h>
19 #include <cob_base_controller_utils/WheelCommands.h>
20 #include <ros/ros.h>
21 
23 std::vector<ros::Time> g_last_ok;
24 double g_threshold;
28 
29 void commandsCallback(const cob_base_controller_utils::WheelCommands::ConstPtr& msg){
30  g_last_ok.resize(msg->steer_target_error.size(), msg->header.stamp);
31 
32  bool valid = true;
33  for(size_t i = 0; i < msg->steer_target_error.size(); ++i){
34 
35  if(fabs(msg->steer_target_error[i]) >= g_threshold){
36  valid = false;
37  if( (msg->header.stamp - g_last_ok[i]) >= g_timeout && !g_stop_requested) {
38  g_stop_requested = true;
39  std_srvs::Trigger srv;
40  ROS_ERROR_STREAM("Wheel " << i << " exceeded threshold for too long, stopping..");
41  g_stop_client.call(srv);
42  }
43  }else{
44  g_last_ok[i] = msg->header.stamp;
45  }
46  }
47 
48  if(valid) g_stop_requested = false;
49 }
50 
51 int main(int argc, char* argv[])
52 {
53  ros::init(argc, argv, "cob_stuck_detector");
54 
55  ros::NodeHandle nh;
56  ros::NodeHandle nh_priv("~");
57 
58  double timeout;
59  if(!nh_priv.getParam("timeout", timeout) || timeout <= 0.0){
60  ROS_ERROR("Please provide valid timeout");
61  return 1;
62  }
63 
64  if(!nh_priv.getParam("threshold", g_threshold)){
65  ROS_ERROR("Please provide stuck threshold");
66  return 1;
67  }
68 
69  if(!nh_priv.getParam("shutdown", g_shutdown)){
70  ROS_ERROR("Please specify whether shutdown (true) or halt (false) is called");
71  return 1;
72  }
73 
74  g_stop_requested = false;
75  g_timeout = ros::Duration(timeout);
76 
77  ros::Subscriber status_sub = nh.subscribe("twist_controller/wheel_commands", 10, commandsCallback);
78  if(g_shutdown){
79  g_stop_client = nh.serviceClient<std_srvs::Trigger>("driver/shutdown");
80  }else{
81  g_stop_client = nh.serviceClient<std_srvs::Trigger>("driver/halt");
82  }
83 
84  ros::spin();
85  return 0;
86 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
double g_threshold
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
bool g_shutdown
ROSCPP_DECL void spin(Spinner &spinner)
void commandsCallback(const cob_base_controller_utils::WheelCommands::ConstPtr &msg)
std::vector< ros::Time > g_last_ok
ros::Duration g_timeout
int main(int argc, char *argv[])
bool getParam(const std::string &key, std::string &s) const
bool g_stop_requested
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)
ros::ServiceClient g_stop_client


cob_base_controller_utils
Author(s): Felix Messmer , Mathias L├╝dtke
autogenerated on Fri Nov 8 2019 03:46:51