Starting with scanner

src/selfie_starting_procedure/include/selfie_starting_procedure/gate_scanner.h

@@ -4,6 +4,7 @@
 #include <ros/ros.h>
 #include <std_srvs/Empty.h>
 #include <vector>
+#include <stdlib.h>
 #include <sensor_msgs/LaserScan.h>
 #include <std_msgs/Empty.h>
 
@@ -13,24 +14,32 @@ class GateScanner
 
   ros::NodeHandle nh_,pnh_;
   ros::Subscriber scan_sub_; 
-  ros::ServiceServer start_serv_;
-  ros::Publisher gate_open_pub_;
+  ros::ServiceServer start_serv_, stop_serv_;
+  ros::Publisher gate_open_pub_, debug_scan_pub_;
 
+  std::vector<float> measurements_;
 
+  // float no_obstacle_time_thresh_;
+  // float min_distance_;
+  // int min_gate_seen_count_;
+  // int gate_seen_count_;
+  float gate_width_;
+  float distance_threshold_;
 
-  float no_obstacle_time_thresh_;
-  float min_width_;
-  float min_distance_;
-  float max_distance_;
-  int min_gate_seen_count_;
-  int gate_seen_count_;
+  int number_of_saved_measurements_;
+  int no_obstacles_limit_;
+  int no_obstacles_counter_{0};
 
   ros::Timer timer_;
 
   bool startSearching(std_srvs::Empty::Request &rq, std_srvs::Empty::Response &rp);
+  bool stopSearching(std_srvs::Empty::Request &rq, std_srvs::Empty::Response &rp);
+  void checkDistances();
+  bool checkValidity();
   void laserScanCB(const sensor_msgs::LaserScan &msg);
-  void timerCallback(const ros::TimerEvent &e);
-  void resetTimer();
+  void appendMeasurement(const float &value);
+  // void timerCallback(const ros::TimerEvent &e);
+  // void resetTimer();
 
   struct Point
   {

src/selfie_starting_procedure/include/selfie_starting_procedure/starting_procedure_action.h

@@ -25,6 +25,8 @@ class StartingProcedureAction
   float starting_speed_;
   bool use_scan_;
   bool use_qr_;
+  bool gate_open_qr_;
+  bool gate_open_scan_;
 
   //create messages that are used to published feedback/result
   selfie_msgs::startingGoal goal_;
@@ -69,8 +71,8 @@ class StartingProcedureAction
   void parkingButtonCB(const std_msgs::Empty &msg);
   void obstacleButtonCB(const std_msgs::Empty &msg);
   void distanceCB(const std_msgs::Float32ConstPtr &msg);
-  void gateOpenCB(const std_msgs::Empty &msg);
-
+  void qrGateOpenCB(const std_msgs::Empty &msg);
+  void scanGateOpenCB(const std_msgs::Empty &msg);
 
 public:
 

src/selfie_starting_procedure/launch/starting_procedure_scan.launch

@@ -7,6 +7,6 @@
     <param name="qr_invisible_time_thresh" type="double" value="2.0"/>
   </node-->
   <node pkg="selfie_starting_procedure" type="gate_scanner" name="gate_scanner" output="screen">
-    <param name="no_obstacle_time_thresh" type="double" value="2.0"/>
+    <param name="no_obstacles_limit" type="double" value="25.0"/>
   </node>
 </launch>

src/selfie_starting_procedure/src/scanner.cpp

@@ -6,60 +6,111 @@
 GateScanner::GateScanner(const ros::NodeHandle &nh, const ros::NodeHandle &pnh):
 nh_(nh), pnh_(pnh)
 {
-  pnh_.param<float>("no_obstacle_time_thresh", no_obstacle_time_thresh_, 1.5f);
-  pnh_.param<float>("min_distance", min_distance_, 0.1);
-  pnh_.param<float>("max_distance", max_distance_, 0.5);
-  pnh_.param<float>("min_width", min_width_, 0.2);
-  pnh_.param<int>("min_gate_seen_count", min_gate_seen_count_, 5);
-  timer_ = nh_.createTimer(ros::Duration(no_obstacle_time_thresh_), &GateScanner::timerCallback, this, true, false);
+  pnh_.param<float>("distance_threshold", distance_threshold_, 1.0);
+  pnh_.param<float>("gate_width", gate_width_, 0.2);
+  pnh_.param<int>("number_of_saved_measurements", number_of_saved_measurements_, 10);
+  pnh_.param<int>("no_obstacles_limit", no_obstacles_limit_, 10);
+
   start_serv_ = nh_.advertiseService("startGateScan", &GateScanner::startSearching, this);
+  stop_serv_ = nh_.advertiseService("stopGateScan", &GateScanner::stopSearching, this);
   gate_open_pub_ = nh_.advertise<std_msgs::Empty>("scan_gate_open", 1);
-  if(min_gate_seen_count_ < 1) min_gate_seen_count_ = 1;
-  gate_seen_count_ = 0;
+
+  // publishing tresholded pointcloud
+  debug_scan_pub_ = nh_.advertise<sensor_msgs::LaserScan>("gate_scanner_debug",10);
+}
+
+void GateScanner::appendMeasurement(const float &value)
+{
+  // too little measurements
+  if (measurements_.size() > number_of_saved_measurements_)
+  {
+    measurements_.erase(measurements_.begin());
+  }
+  measurements_.push_back(value);
+}
+
+void GateScanner::checkDistances()
+{
+  // if mean of all measurements is greater than threshold
+  if ((std::accumulate(measurements_.begin(), measurements_.end(), 0.0) /
+                      measurements_.size()) > distance_threshold_ &&
+       measurements_.back() > distance_threshold_)
+  {
+    no_obstacles_counter_++;
+  }
+  else 
+  {
+    no_obstacles_counter_ = 0;
+  }
+}
+
+bool GateScanner::checkValidity(){
+  return (no_obstacles_counter_ >= no_obstacles_limit_) ? true : false;
 }
 
 void GateScanner::laserScanCB(const sensor_msgs::LaserScan &msg)
 {
+  std::vector<float> valid_measurement;
+  ROS_INFO("msg size: %d", static_cast<int>(msg.ranges.size()));
+  sensor_msgs::LaserScan debug_msg = msg;
+  debug_msg.ranges.clear();
   float angle = msg.angle_min;
   for (std::vector<float>::const_iterator it = msg.ranges.begin(); it < msg.ranges.end();
    ++it, angle += msg.angle_increment)
   {
+    // out of bounds
     if (*it > msg.range_max || *it < msg.range_min) continue;
+    // converting to cartesian plane
     Point point(angle, *it);
     {
-      if (point.x > min_distance_ && point.x < max_distance_ && std::abs(point.y) < min_width_ / 2.f)
+      // checking if measurement is in gate range
+      if (std::abs(point.y) < (gate_width_ / 2.f))
       {
-        if(++gate_seen_count_ == min_gate_seen_count_)
-        {
-          timer_.start();
-        }
-        else if(gate_seen_count_ > min_gate_seen_count_)
-        {
-          resetTimer();
-        }
-        return;
-
+        // finding measurement inside gate range
+        valid_measurement.push_back(point.x);
+        // publishing debug pointcloud
+        debug_msg.ranges.push_back(*it);
+        ROS_INFO("Vector data: %f", *it);
       }
     }
   }
+  // if lidar doesnt produce any data in this direction it means 
+  //gate is opened
+  if (valid_measurement.size() == 0) 
+  {
+    no_obstacles_counter_++;
+    ROS_INFO("Gate Scanner: counter state %d", no_obstacles_counter_);
+    return;
+  }
+  float mean = std::accumulate(valid_measurement.begin(), valid_measurement.end(), 0.0) / 
+                               valid_measurement.size();
+  ROS_INFO("Measurement mean: %f", mean);
+  debug_scan_pub_.publish(debug_msg);
+  appendMeasurement(mean);
+  // check if measurements are greater than treshold distance
+  checkDistances();
+  // check if gate is open
+  if (checkValidity()) 
+  {
+    gate_open_pub_.publish(std_msgs::Empty());
+    ROS_INFO("GateScanner: gate open");
+    no_obstacles_counter_ = 0;
+    scan_sub_.shutdown(); 
+  }
+  ROS_INFO("Gate Scanner: counter state %d", no_obstacles_counter_);
 }
 
-void GateScanner::timerCallback(const ros::TimerEvent &e)
-{
-  gate_open_pub_.publish(std_msgs::Empty());
-  ROS_INFO("timer callback");
-  scan_sub_.shutdown();
-}
-
-void GateScanner::resetTimer()
+bool GateScanner::startSearching(std_srvs::Empty::Request &rq, std_srvs::Empty::Response &rp)
 {
-  timer_.setPeriod(ros::Duration(no_obstacle_time_thresh_), true);  // reset
-  ROS_INFO("timer reset");
+  scan_sub_ = nh_.subscribe("scan", 10, &GateScanner::laserScanCB, this);
+  ROS_INFO("GateScanner: start searching");
+  return true;
 }
 
-bool GateScanner::startSearching(std_srvs::Empty::Request &rq, std_srvs::Empty::Response &rp)
+bool GateScanner::stopSearching(std_srvs::Empty::Request &rq, std_srvs::Empty::Response &rp)
 {
-  scan_sub_ = nh_.subscribe("scan", 10, &GateScanner::laserScanCB, this);
+  scan_sub_.shutdown();
+  ROS_INFO("GateScanner: stopped searching");
   return true;
 }
 

src/selfie_starting_procedure/src/starting_procedure_action.cpp

@@ -7,7 +7,8 @@
 StartingProcedureAction::StartingProcedureAction(const ros::NodeHandle &nh, const ros::NodeHandle &pnh) :
   nh_(nh), pnh_(pnh), as_(nh_, "starting_procedure",  false),
   min_second_press_time_(ros::Time(0)), debounce_duration_(ros::Duration(2)),
-  distance_goal_(0.f), distance_read_(0.f)
+  distance_goal_(0.f), distance_read_(0.f), gate_open_qr_(false),
+  gate_open_scan_(false)
 {
   as_.registerPreemptCallback(boost::bind(&StartingProcedureAction::preemptCB, this));
   as_.registerGoalCallback(boost::bind(&StartingProcedureAction::executeCB, this));
@@ -34,11 +35,11 @@ void StartingProcedureAction::executeCB()
   ROS_INFO("received goal %f", goal.distance);
   if (use_qr_)
   {
-    qr_sub_ = nh_.subscribe("qr_gate_open", 1, &StartingProcedureAction::gateOpenCB, this);
+    qr_sub_ = nh_.subscribe("qr_gate_open", 1, &StartingProcedureAction::qrGateOpenCB, this);
   }
   if (use_scan_)
   {
-    gate_scan_sub_ = nh_.subscribe("scan_gate_open", 1, &StartingProcedureAction::gateOpenCB, this);
+    gate_scan_sub_ = nh_.subscribe("scan_gate_open", 1, &StartingProcedureAction::scanGateOpenCB, this);
   }
   distance_sub_ = nh_.subscribe("distance", 10, &StartingProcedureAction::distanceCB, this);
   parking_button_sub_ = nh_.subscribe("start_button1", 10, &StartingProcedureAction::parkingButtonCB, this);
@@ -78,6 +79,7 @@ void StartingProcedureAction::parkingButtonCB(const std_msgs::Empty &msg)
         std_srvs::Empty call = std_srvs::Empty();
         qr_stop_search_.call(call);
       }
+      // TODO scan stop search
       publishFeedback(BUTTON_PARKING_DRIVE_PRESSED);
       distance_goal_ = distance_read_ + distance_goal_;
       starting_distance_ = distance_read_;
@@ -129,9 +131,44 @@ void StartingProcedureAction::obstacleButtonCB(const std_msgs::Empty &msg)
   }
 }
 
-void StartingProcedureAction::gateOpenCB(const std_msgs::Empty &msg)
+void StartingProcedureAction::qrGateOpenCB(const std_msgs::Empty &msg)
 {
   if (state_ == State::WAIT_START)
+  {
+    // nie uzywamy skanu
+    if(!use_scan_)
+    {
+      state_ = State::START_MOVE;
+      publishFeedback(button_status_);
+      distance_goal_ = distance_read_ + distance_goal_;
+      ROS_INFO("gate was opened");
+      starting_distance_ = distance_read_;
+      publishFeedback(START_DRIVE);
+    }
+    else // uzywamy skanu
+    {
+      // jezeli skan jeszcze nie dal znaku ze dziala
+      // to oznaczamy ze qr pokazal ze jest git
+      if (!gate_open_scan_) {
+        gate_open_qr_ = true;
+      }
+      else // qr tez pokazal ze gate otwarty
+      {
+      state_ = State::START_MOVE;
+      publishFeedback(button_status_);
+      distance_goal_ = distance_read_ + distance_goal_;
+      ROS_INFO("gate was opened");
+      starting_distance_ = distance_read_;
+      publishFeedback(START_DRIVE);
+      }
+    }
+  }
+}
+
+void StartingProcedureAction::scanGateOpenCB(const std_msgs::Empty &msg)
+{
+  // nie uzywamy qr
+  if(!use_qr_)
   {
     state_ = State::START_MOVE;
     publishFeedback(button_status_);
@@ -140,6 +177,23 @@ void StartingProcedureAction::gateOpenCB(const std_msgs::Empty &msg)
     starting_distance_ = distance_read_;
     publishFeedback(START_DRIVE);
   }
+  else // uzywamy qr
+  {
+    // jezeli skan jeszcze nie dal znaku ze dziala
+    // to oznaczamy ze qr pokazal ze jest gitówka
+    if (!gate_open_qr_) {
+      gate_open_scan_ = true;
+    }
+    else // skan tez pokazal ze gate otwarty
+    {
+    state_ = State::START_MOVE;
+    publishFeedback(button_status_);
+    distance_goal_ = distance_read_ + distance_goal_;
+    ROS_INFO("gate was opened");
+    starting_distance_ = distance_read_;
+    publishFeedback(START_DRIVE);
+    }
+  }
 }
 
 void StartingProcedureAction::preemptCB()