safety_controller.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, Yujin Robot.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of Yujin Robot nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
41 /*****************************************************************************
42 ** Ifdefs
43 *****************************************************************************/
44 
45 #ifndef SAFETY_CONTROLLER_HPP_
46 #define SAFETY_CONTROLLER_HPP_
47 
48 /*****************************************************************************
49 ** Includes
50 *****************************************************************************/
51 
52 #include <string>
53 #include <ros/ros.h>
55 #include <std_msgs/Empty.h>
56 #include <geometry_msgs/Twist.h>
57 #include <roch_msgs/CliffEvent.h>
58 #include <roch_msgs/UltEvent.h>
59 #include <roch_msgs/PSDEvent.h>
60 
61 namespace sawyer
62 {
63 
76 {
77 public:
78  SafetyController(ros::NodeHandle& nh, std::string& name) :
79  Controller(),
80  nh_(nh),
81  name_(name),
82  cliff_left_detected_(false),
83  cliff_right_detected_(false),
84  ult_left_detected_(false),
85  ult_center_detected_(false),
86  ult_right_detected_(false),
87  psd_left_detected_(false),
88  psd_center_detected_(false),
89  psd_right_detected_(false),
90  last_event_time_(ros::Time(0)),
91  msg_(new geometry_msgs::Twist()){};
93 
98  bool init()
99  {
100  //how long to keep sending messages after a bump, cliff, or wheel drop stops
101  double time_to_extend_ult_cliff_psd_events;
102  nh_.param("time_to_extend_ult_cliff_psd_events", time_to_extend_ult_cliff_psd_events, 0.0);
103  time_to_extend_bump_cliff_events_ = ros::Duration(time_to_extend_ult_cliff_psd_events);
110  velocity_command_publisher_ = nh_.advertise< geometry_msgs::Twist >("cmd_vel", 10);
111  return true;
112  };
113 
117  void spin();
118 
119 private:
121  std::string name_;
131 
132  geometry_msgs::TwistPtr msg_; // velocity command
133 
138  void enableCB(const std_msgs::EmptyConstPtr msg);
139 
144  void disableCB(const std_msgs::EmptyConstPtr msg);
145 
146 
151  void cliffEventCB(const roch_msgs::CliffEventConstPtr msg);
152 
153 
158  void ultEventCB(const roch_msgs::UltEventConstPtr msg);
159 
160 
165  void psdEventCB(const roch_msgs::PSDEventConstPtr msg);
166 
167 
176  void resetSafetyStatesCB(const std_msgs::EmptyConstPtr msg);
177 };
178 
179 
180 void SafetyController::enableCB(const std_msgs::EmptyConstPtr msg)
181 {
182  if (this->enable())
183  {
184  ROS_INFO_STREAM("Controller has been enabled. [" << name_ << "]");
185  }
186  else
187  {
188  ROS_INFO_STREAM("Controller was already enabled. [" << name_ <<"]");
189  }
190 };
191 
192 void SafetyController::disableCB(const std_msgs::EmptyConstPtr msg)
193 {
194  if (this->disable())
195  {
196  ROS_INFO_STREAM("Controller has been disabled. [" << name_ <<"]");
197  }
198  else
199  {
200  ROS_INFO_STREAM("Controller was already disabled. [" << name_ <<"]");
201  }
202 };
203 
204 void SafetyController::cliffEventCB(const roch_msgs::CliffEventConstPtr msg)
205 {
206  if ((msg->leftState == roch_msgs::CliffEvent::CLIFF) || (msg->rightState == roch_msgs::CliffEvent::CLIFF) )
207  {
209  ROS_DEBUG_STREAM("Cliff detected. Moving backwards. [" << name_ << "]");
210  if((msg->leftState == roch_msgs::CliffEvent::CLIFF))
211  cliff_left_detected_ = true;
212  if((msg->rightState == roch_msgs::CliffEvent::CLIFF))
213  cliff_right_detected_ = true;
214  }
215  else // roch_msgs::CliffEvent::FLOOR
216  {
217  ROS_DEBUG_STREAM("Not detecting any cliffs. Resuming normal operation. [" << name_ << "]");
218  if((msg->leftState == roch_msgs::CliffEvent::FLOOR))
219  cliff_left_detected_ = false;
220  if((msg->rightState == roch_msgs::CliffEvent::FLOOR))
221  cliff_right_detected_ = false;
222  }
223 };
224 
225 void SafetyController::ultEventCB(const roch_msgs::UltEventConstPtr msg)
226 {
227  if ((msg->leftState == roch_msgs::UltEvent::NEAR) || (msg->rightState == roch_msgs::UltEvent::NEAR) || (msg->centerState == roch_msgs::UltEvent::NEAR) )
228  {
230  ROS_DEBUG_STREAM("Ult detected. Moving backwards. [" << name_ << "]");
231  if((msg->leftState == roch_msgs::UltEvent::NEAR))
232  ult_left_detected_ = true;
233  if((msg->centerState == roch_msgs::UltEvent::NEAR))
234  ult_center_detected_ = true;
235  if((msg->rightState == roch_msgs::UltEvent::NEAR))
236  ult_right_detected_ = true;
237  }
238  else // roch_msgs::UltEvent::NORMAL
239  {
240  ROS_DEBUG_STREAM("Not detecting any cliffs. Resuming normal operation. [" << name_ << "]");
241  if((msg->leftState == roch_msgs::UltEvent::NORMAL))
242  ult_left_detected_ = false;
243  if((msg->centerState == roch_msgs::UltEvent::NORMAL))
244  ult_center_detected_ = false;
245  if((msg->rightState == roch_msgs::UltEvent::NORMAL))
246  ult_right_detected_ = false;
247  }
248 };
249 
250 
251 void SafetyController::psdEventCB(const roch_msgs::PSDEventConstPtr msg)
252 {
253  if ((msg->leftState == roch_msgs::PSDEvent::NEAR) || (msg->rightState == roch_msgs::PSDEvent::NEAR) || (msg->centerState == roch_msgs::PSDEvent::NEAR) )
254  {
256  ROS_DEBUG_STREAM("PSD detected. Moving backwards. [" << name_ << "]");
257  if((msg->leftState == roch_msgs::PSDEvent::NEAR))
258  psd_left_detected_ = true;
259  if((msg->centerState == roch_msgs::PSDEvent::NEAR))
260  psd_center_detected_ = true;
261  if((msg->rightState == roch_msgs::PSDEvent::NEAR))
262  psd_right_detected_ = true;
263  }
264  else // roch_msgs::PSDEvent::NORMAL
265  {
266  ROS_DEBUG_STREAM("Not detecting any cliffs. Resuming normal operation. [" << name_ << "]");
267  if((msg->leftState == roch_msgs::PSDEvent::NORMAL))
268  psd_left_detected_ = false;
269  if((msg->centerState == roch_msgs::PSDEvent::NORMAL))
270  psd_center_detected_ = false;
271  if((msg->rightState == roch_msgs::PSDEvent::NORMAL))
272  psd_right_detected_ = false;
273  }
274 };
275 
276 
277 void SafetyController::resetSafetyStatesCB(const std_msgs::EmptyConstPtr msg)
278 {
279  cliff_left_detected_ = false;
280  cliff_right_detected_ = false;
281  ult_left_detected_ = false;
282  ult_center_detected_ = false;
283  ult_right_detected_ = false;
284  psd_left_detected_ = false;
285  psd_center_detected_ = false;
286  psd_right_detected_ = false;
287  ROS_WARN_STREAM("All safety states have been reset to false. [" << name_ << "]");
288 }
289 
291 {
292  if (this->getState())
293  {
294  if ( psd_center_detected_ || ult_center_detected_) //ult_center_detected_: disable due to the ult not stable
295  {
296  msg_.reset(new geometry_msgs::Twist());
297  msg_->linear.x = -0.1;
298  msg_->linear.y = 0.0;
299  msg_->linear.z = 0.0;
300  msg_->angular.x = 0.0;
301  msg_->angular.y = 0.0;
302  msg_->angular.z = 0.0;
304  }
305  else if (cliff_left_detected_ || psd_left_detected_ || ult_left_detected_)//ult_left_detected_: ult_center_detected_ disable due to the ult not stable
306  {
307  // left bump/cliff; also spin a bit to the right to make escape easier
308  msg_.reset(new geometry_msgs::Twist());
309  msg_->linear.x = -0.1;
310  msg_->linear.y = 0.0;
311  msg_->linear.z = 0.0;
312  msg_->angular.x = 0.0;
313  msg_->angular.y = 0.0;
314  msg_->angular.z = -0.4;
316  }
317  else if (cliff_right_detected_ || psd_right_detected_ || ult_right_detected_)//ult_right_detected_: ult_center_detected_ disable due to the ult not stable
318  {
319  // right bump/cliff; also spin a bit to the left to make escape easier
320  msg_.reset(new geometry_msgs::Twist());
321  msg_->linear.x = -0.1;
322  msg_->linear.y = 0.0;
323  msg_->linear.z = 0.0;
324  msg_->angular.x = 0.0;
325  msg_->angular.y = 0.0;
326  msg_->angular.z = 0.4;
328  }
329  //if we want to extend the safety state and we're within the time, just keep sending msg_
333  }
334  }
335 };
336 
337 } // namespace sawyer
338 
339 #endif /* SAFETY_CONTROLLER_HPP_ */
void ultEventCB(const roch_msgs::UltEventConstPtr msg)
Keeps track of ult detection.
void psdEventCB(const roch_msgs::PSDEventConstPtr msg)
Keeps track of PSD detection.
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::Subscriber enable_controller_subscriber_
ros::Subscriber ult_event_subscriber_
ros::Duration time_to_extend_bump_cliff_events_
geometry_msgs::TwistPtr msg_
void cliffEventCB(const roch_msgs::CliffEventConstPtr msg)
Keeps track of cliff detection.
ros::Subscriber psd_event_subscriber_
SafetyController(ros::NodeHandle &nh, std::string &name)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::Subscriber cliff_event_subscriber_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher velocity_command_publisher_
void disableCB(const std_msgs::EmptyConstPtr msg)
ROS logging output for disabling the controller.
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
void resetSafetyStatesCB(const std_msgs::EmptyConstPtr msg)
Callback for resetting safety variables.
ros::Subscriber reset_safety_states_subscriber_
ros::Publisher controller_state_publisher_
ros::Subscriber disable_controller_subscriber_
#define ROS_INFO_STREAM(args)
static Time now()
void enableCB(const std_msgs::EmptyConstPtr msg)
ROS logging output for enabling the controller.


roch_safety_controller
Author(s): Marcus Liebhardt , Chen
autogenerated on Mon Jun 10 2019 14:41:24