safety_controller.hpp

Go to the documentation of this file.

#ifndef YOCS_SAFETY_CONTROLLER_HPP_
#define YOCS_SAFETY_CONTROLLER_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <string>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <sensor_msgs/Range.h>
#include <yocs_controllers/default_controller.hpp>

namespace yocs_safety_controller
{

class SafetyController : public yocs::Controller
{
public:
  SafetyController(ros::NodeHandle& nh_priv, const std::string& name);
  ~SafetyController(){};

  bool init();

  void spinOnce();


protected:
  ros::NodeHandle nh_priv_;
  std::string name_;
  ros::Subscriber enable_controller_subscriber_, disable_controller_subscriber_, ranger_subscriber_;
  ros::Publisher controller_state_publisher_, velocity_command_publisher_;
  geometry_msgs::TwistPtr cmd_vel_msg_; // velocity command

  bool obstacle_detected_;
  bool reverse_;
  bool reversing_;
  ros::Time reversing_start_;
  double reversing_distance_;
  double reversing_velocity_;
  ros::Duration reversing_duration_;

  void enableCB(const std_msgs::EmptyConstPtr msg);

  void disableCB(const std_msgs::EmptyConstPtr msg);

  void rangerCB(const sensor_msgs::RangeConstPtr msg);

};

} // namespace yocs_safety_controller

#endif // YOCS_SAFETY_CONTROLLER_HPP_