safety_controller.hpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Yujin Robot.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Yujin Robot nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00041 /*****************************************************************************
00042 ** Ifdefs
00043 *****************************************************************************/
00044 
00045 #ifndef SAFETY_CONTROLLER_HPP_
00046 #define SAFETY_CONTROLLER_HPP_
00047 
00048 /*****************************************************************************
00049 ** Includes
00050 *****************************************************************************/
00051 
00052 #include <string>
00053 #include <ros/ros.h>
00054 #include <yocs_controllers/default_controller.hpp>
00055 #include <std_msgs/Empty.h>
00056 #include <geometry_msgs/Twist.h>
00057 #include <kobuki_msgs/BumperEvent.h>
00058 #include <kobuki_msgs/CliffEvent.h>
00059 #include <kobuki_msgs/WheelDropEvent.h>
00060 
00061 namespace kobuki
00062 {
00063 
00075 class SafetyController : public yocs::Controller
00076 {
00077 public:
00078   SafetyController(ros::NodeHandle& nh, std::string& name) :
00079     Controller(),
00080     nh_(nh),
00081     name_(name),
00082     wheel_left_dropped_(false),
00083     wheel_right_dropped_(false),
00084     bumper_left_pressed_(false),
00085     bumper_center_pressed_(false),
00086     bumper_right_pressed_(false),
00087     cliff_left_detected_(false),
00088     cliff_center_detected_(false),
00089     cliff_right_detected_(false), 
00090     last_event_time_(ros::Time(0)),
00091     msg_(new geometry_msgs::Twist()){};
00092   ~SafetyController(){};
00093 
00098   bool init()
00099   {
00100     //how long to keep sending messages after a bump, cliff, or wheel drop stops
00101     double time_to_extend_bump_cliff_events;
00102     nh_.param("time_to_extend_bump_cliff_events", time_to_extend_bump_cliff_events, 0.0);
00103     time_to_extend_bump_cliff_events_ = ros::Duration(time_to_extend_bump_cliff_events);
00104     enable_controller_subscriber_ = nh_.subscribe("enable", 10, &SafetyController::enableCB, this);
00105     disable_controller_subscriber_ = nh_.subscribe("disable", 10, &SafetyController::disableCB, this);
00106     bumper_event_subscriber_ = nh_.subscribe("events/bumper", 10, &SafetyController::bumperEventCB, this);
00107     cliff_event_subscriber_  = nh_.subscribe("events/cliff",  10, &SafetyController::cliffEventCB, this);
00108     wheel_event_subscriber_  = nh_.subscribe("events/wheel_drop", 10, &SafetyController::wheelEventCB, this);
00109     reset_safety_states_subscriber_ = nh_.subscribe("reset", 10, &SafetyController::resetSafetyStatesCB, this);
00110     velocity_command_publisher_ = nh_.advertise< geometry_msgs::Twist >("cmd_vel", 10);
00111     return true;
00112   };
00113 
00117   void spin();
00118 
00119 private:
00120   ros::NodeHandle nh_;
00121   std::string name_;
00122   ros::Subscriber enable_controller_subscriber_, disable_controller_subscriber_;
00123   ros::Subscriber bumper_event_subscriber_, cliff_event_subscriber_, wheel_event_subscriber_;
00124   ros::Subscriber reset_safety_states_subscriber_;
00125   ros::Publisher controller_state_publisher_, velocity_command_publisher_;
00126   bool wheel_left_dropped_, wheel_right_dropped_;
00127   bool bumper_left_pressed_, bumper_center_pressed_, bumper_right_pressed_;
00128   bool cliff_left_detected_, cliff_center_detected_, cliff_right_detected_;
00129   ros::Duration time_to_extend_bump_cliff_events_;
00130   ros::Time last_event_time_;
00131 
00132   geometry_msgs::TwistPtr msg_; // velocity command
00133 
00138   void enableCB(const std_msgs::EmptyConstPtr msg);
00139 
00144   void disableCB(const std_msgs::EmptyConstPtr msg);
00145 
00150   void bumperEventCB(const kobuki_msgs::BumperEventConstPtr msg);
00151 
00156   void cliffEventCB(const kobuki_msgs::CliffEventConstPtr msg);
00157 
00162   void wheelEventCB(const kobuki_msgs::WheelDropEventConstPtr msg);
00163 
00172   void resetSafetyStatesCB(const std_msgs::EmptyConstPtr msg);
00173 };
00174 
00175 
00176 void SafetyController::enableCB(const std_msgs::EmptyConstPtr msg)
00177 {
00178   if (this->enable())
00179   {
00180     ROS_INFO_STREAM("Controller has been enabled. [" << name_ << "]");
00181   }
00182   else
00183   {
00184     ROS_INFO_STREAM("Controller was already enabled. [" << name_ <<"]");
00185   }
00186 };
00187 
00188 void SafetyController::disableCB(const std_msgs::EmptyConstPtr msg)
00189 {
00190   if (this->disable())
00191   {
00192     ROS_INFO_STREAM("Controller has been disabled. [" << name_ <<"]");
00193   }
00194   else
00195   {
00196     ROS_INFO_STREAM("Controller was already disabled. [" << name_ <<"]");
00197   }
00198 };
00199 
00200 void SafetyController::cliffEventCB(const kobuki_msgs::CliffEventConstPtr msg)
00201 {
00202   if (msg->state == kobuki_msgs::CliffEvent::CLIFF)
00203   {
00204     last_event_time_ = ros::Time::now();
00205     ROS_DEBUG_STREAM("Cliff detected. Moving backwards. [" << name_ << "]");
00206     switch (msg->sensor)
00207     {
00208       case kobuki_msgs::CliffEvent::LEFT:    cliff_left_detected_   = true;  break;
00209       case kobuki_msgs::CliffEvent::CENTER:  cliff_center_detected_ = true;  break;
00210       case kobuki_msgs::CliffEvent::RIGHT:   cliff_right_detected_  = true;  break;
00211     }
00212   }
00213   else // kobuki_msgs::CliffEvent::FLOOR
00214   {
00215     ROS_DEBUG_STREAM("Not detecting any cliffs. Resuming normal operation. [" << name_ << "]");
00216     switch (msg->sensor)
00217     {
00218       case kobuki_msgs::CliffEvent::LEFT:    cliff_left_detected_   = false; break;
00219       case kobuki_msgs::CliffEvent::CENTER:  cliff_center_detected_ = false; break;
00220       case kobuki_msgs::CliffEvent::RIGHT:   cliff_right_detected_  = false; break;
00221     }
00222   }
00223 };
00224 
00225 void SafetyController::bumperEventCB(const kobuki_msgs::BumperEventConstPtr msg)
00226 {
00227   if (msg->state == kobuki_msgs::BumperEvent::PRESSED)
00228   {
00229     last_event_time_ = ros::Time::now();
00230     ROS_DEBUG_STREAM("Bumper pressed. Moving backwards. [" << name_ << "]");
00231     switch (msg->bumper)
00232     {
00233       case kobuki_msgs::BumperEvent::LEFT:    bumper_left_pressed_   = true;  break;
00234       case kobuki_msgs::BumperEvent::CENTER:  bumper_center_pressed_ = true;  break;
00235       case kobuki_msgs::BumperEvent::RIGHT:   bumper_right_pressed_  = true;  break;
00236     }
00237   }
00238   else // kobuki_msgs::BumperEvent::RELEASED
00239   {
00240     ROS_DEBUG_STREAM("No bumper pressed. Resuming normal operation. [" << name_ << "]");
00241     switch (msg->bumper)
00242     {
00243       case kobuki_msgs::BumperEvent::LEFT:    bumper_left_pressed_   = false; break;
00244       case kobuki_msgs::BumperEvent::CENTER:  bumper_center_pressed_ = false; break;
00245       case kobuki_msgs::BumperEvent::RIGHT:   bumper_right_pressed_  = false; break;
00246     }
00247   }
00248 };
00249 
00250 void SafetyController::wheelEventCB(const kobuki_msgs::WheelDropEventConstPtr msg)
00251 {
00252   if (msg->state == kobuki_msgs::WheelDropEvent::DROPPED)
00253   {
00254     // need to keep track of both wheels separately
00255     if (msg->wheel == kobuki_msgs::WheelDropEvent::LEFT)
00256     {
00257       ROS_DEBUG_STREAM("Left wheel dropped. [" << name_ << "]");
00258       wheel_left_dropped_ = true;
00259     }
00260     else // kobuki_msgs::WheelDropEvent::RIGHT
00261     {
00262       ROS_DEBUG_STREAM("Right wheel dropped. [" << name_ << "]");
00263       wheel_right_dropped_ = true;
00264     }
00265   }
00266   else // kobuki_msgs::WheelDropEvent::RAISED
00267   {
00268     // need to keep track of both wheels separately
00269     if (msg->wheel == kobuki_msgs::WheelDropEvent::LEFT)
00270     {
00271       ROS_DEBUG_STREAM("Left wheel raised. [" << name_ << "]");
00272       wheel_left_dropped_ = false;
00273     }
00274     else // kobuki_msgs::WheelDropEvent::RIGHT
00275     {
00276       ROS_DEBUG_STREAM("Right wheel raised. [" << name_ << "]");
00277       wheel_right_dropped_ = false;
00278     }
00279     if (!wheel_left_dropped_ && !wheel_right_dropped_)
00280     {
00281       ROS_DEBUG_STREAM("Both wheels raised. Resuming normal operation. [" << name_ << "]");
00282     }
00283   }
00284 };
00285 
00286 void SafetyController::resetSafetyStatesCB(const std_msgs::EmptyConstPtr msg)
00287 {
00288   wheel_left_dropped_    = false;
00289   wheel_right_dropped_   = false;
00290   bumper_left_pressed_   = false;
00291   bumper_center_pressed_ = false;
00292   bumper_right_pressed_  = false;
00293   cliff_left_detected_   = false;
00294   cliff_center_detected_ = false;
00295   cliff_right_detected_  = false;
00296   ROS_WARN_STREAM("All safety states have been reset to false. [" << name_ << "]");
00297 }
00298 
00299 void SafetyController::spin()
00300 {
00301   if (this->getState())
00302   {
00303     if (wheel_left_dropped_ || wheel_right_dropped_)
00304     {
00305       msg_.reset(new geometry_msgs::Twist());
00306       msg_->linear.x = 0.0;
00307       msg_->linear.y = 0.0;
00308       msg_->linear.z = 0.0;
00309       msg_->angular.x = 0.0;
00310       msg_->angular.y = 0.0;
00311       msg_->angular.z = 0.0;
00312       velocity_command_publisher_.publish(msg_);
00313     }
00314     else if (bumper_center_pressed_ || cliff_center_detected_)
00315     {
00316       msg_.reset(new geometry_msgs::Twist());
00317       msg_->linear.x = -0.1;
00318       msg_->linear.y = 0.0;
00319       msg_->linear.z = 0.0;
00320       msg_->angular.x = 0.0;
00321       msg_->angular.y = 0.0;
00322       msg_->angular.z = 0.0;
00323       velocity_command_publisher_.publish(msg_);
00324     }
00325     else if (bumper_left_pressed_ || cliff_left_detected_)
00326     {
00327       // left bump/cliff; also spin a bit to the right to make escape easier
00328       msg_.reset(new geometry_msgs::Twist());
00329       msg_->linear.x = -0.1;
00330       msg_->linear.y = 0.0;
00331       msg_->linear.z = 0.0;
00332       msg_->angular.x = 0.0;
00333       msg_->angular.y = 0.0;
00334       msg_->angular.z = -0.4;
00335       velocity_command_publisher_.publish(msg_);
00336     }
00337     else if (bumper_right_pressed_ || cliff_right_detected_)
00338     {
00339       // right bump/cliff; also spin a bit to the left to make escape easier
00340       msg_.reset(new geometry_msgs::Twist());
00341       msg_->linear.x = -0.1;
00342       msg_->linear.y = 0.0;
00343       msg_->linear.z = 0.0;
00344       msg_->angular.x = 0.0;
00345       msg_->angular.y = 0.0;
00346       msg_->angular.z = 0.4;
00347       velocity_command_publisher_.publish(msg_);
00348     }
00349     //if we want to extend the safety state and we're within the time, just keep sending msg_
00350     else if (time_to_extend_bump_cliff_events_ > ros::Duration(1e-10) && 
00351              ros::Time::now() - last_event_time_ < time_to_extend_bump_cliff_events_) {
00352       velocity_command_publisher_.publish(msg_);
00353     }
00354   }
00355 };
00356 
00357 } // namespace kobuki
00358 
00359 #endif /* SAFETY_CONTROLLER_HPP_ */


kobuki_safety_controller
Author(s): Marcus Liebhardt
autogenerated on Thu Jun 6 2019 17:37:52