From b1f53a5a343a94b7573049f5b48667d0564f42cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Sun, 9 Feb 2020 21:32:28 +0100 Subject: [PATCH 1/2] Lidar detecting added --- .../selfie_starting_procedure/gate_scanner.h | 29 +++-- .../launch/starting_procedure_scan.launch | 1 + src/selfie_starting_procedure/src/scanner.cpp | 113 +++++++++++++----- 3 files changed, 102 insertions(+), 41 deletions(-) diff --git a/src/selfie_starting_procedure/include/selfie_starting_procedure/gate_scanner.h b/src/selfie_starting_procedure/include/selfie_starting_procedure/gate_scanner.h index 3f154c8..4693fc4 100644 --- a/src/selfie_starting_procedure/include/selfie_starting_procedure/gate_scanner.h +++ b/src/selfie_starting_procedure/include/selfie_starting_procedure/gate_scanner.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -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 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 { diff --git a/src/selfie_starting_procedure/launch/starting_procedure_scan.launch b/src/selfie_starting_procedure/launch/starting_procedure_scan.launch index e136053..87c7864 100644 --- a/src/selfie_starting_procedure/launch/starting_procedure_scan.launch +++ b/src/selfie_starting_procedure/launch/starting_procedure_scan.launch @@ -8,5 +8,6 @@ + diff --git a/src/selfie_starting_procedure/src/scanner.cpp b/src/selfie_starting_procedure/src/scanner.cpp index 2435710..a691430 100644 --- a/src/selfie_starting_procedure/src/scanner.cpp +++ b/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("no_obstacle_time_thresh", no_obstacle_time_thresh_, 1.5f); - pnh_.param("min_distance", min_distance_, 0.1); - pnh_.param("max_distance", max_distance_, 0.5); - pnh_.param("min_width", min_width_, 0.2); - pnh_.param("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("distance_threshold", distance_threshold_, 1.0); + pnh_.param("gate_width", gate_width_, 0.2); + pnh_.param("number_of_saved_measurements", number_of_saved_measurements_, 10); + pnh_.param("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("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("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 valid_measurement; + ROS_INFO("msg size: %d", static_cast(msg.ranges.size())); + sensor_msgs::LaserScan debug_msg = msg; + debug_msg.ranges.clear(); float angle = msg.angle_min; for (std::vector::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; } From 68fa6cd204970da4ac678f5193dc068f7dfa1d0d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Sun, 9 Feb 2020 22:22:37 +0100 Subject: [PATCH 2/2] Partial starting procedure makeover --- .../starting_procedure_action.h | 6 +- .../launch/starting_procedure_scan.launch | 1 - .../src/starting_procedure_action.cpp | 62 +++++++++++++++++-- 3 files changed, 62 insertions(+), 7 deletions(-) diff --git a/src/selfie_starting_procedure/include/selfie_starting_procedure/starting_procedure_action.h b/src/selfie_starting_procedure/include/selfie_starting_procedure/starting_procedure_action.h index 725f352..2523d33 100644 --- a/src/selfie_starting_procedure/include/selfie_starting_procedure/starting_procedure_action.h +++ b/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: diff --git a/src/selfie_starting_procedure/launch/starting_procedure_scan.launch b/src/selfie_starting_procedure/launch/starting_procedure_scan.launch index 87c7864..031021c 100644 --- a/src/selfie_starting_procedure/launch/starting_procedure_scan.launch +++ b/src/selfie_starting_procedure/launch/starting_procedure_scan.launch @@ -7,7 +7,6 @@ - diff --git a/src/selfie_starting_procedure/src/starting_procedure_action.cpp b/src/selfie_starting_procedure/src/starting_procedure_action.cpp index 810d81d..83e6bca 100644 --- a/src/selfie_starting_procedure/src/starting_procedure_action.cpp +++ b/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()