From 366264294ad0f06023499bde9904f05988096c51 Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Sat, 30 Jan 2021 19:58:22 -0500 Subject: [PATCH 01/13] Removed as much of the stop bar detector as posssible without reworking publishing of move --- rr_iarrc/CMakeLists.txt | 1 + .../launch/perception/stopbar_detector.launch | 15 ++ rr_iarrc/src/stopbar_detector/CMakeLists.txt | 2 + .../src/stopbar_detector/stopbar_detector.cpp | 196 ++++++++++++++++++ 4 files changed, 214 insertions(+) create mode 100644 rr_iarrc/launch/perception/stopbar_detector.launch create mode 100644 rr_iarrc/src/stopbar_detector/CMakeLists.txt create mode 100644 rr_iarrc/src/stopbar_detector/stopbar_detector.cpp diff --git a/rr_iarrc/CMakeLists.txt b/rr_iarrc/CMakeLists.txt index 3b51c3824..2c1fbffb0 100644 --- a/rr_iarrc/CMakeLists.txt +++ b/rr_iarrc/CMakeLists.txt @@ -40,4 +40,5 @@ add_subdirectory(src/cone_detection) add_subdirectory(src/urc_controller) add_subdirectory(src/turning_test) add_subdirectory(src/sign_detector) +add_subdirectory(src/stopbar_detector) add_subdirectory(src/urc_image_merger) diff --git a/rr_iarrc/launch/perception/stopbar_detector.launch b/rr_iarrc/launch/perception/stopbar_detector.launch new file mode 100644 index 000000000..a76d52889 --- /dev/null +++ b/rr_iarrc/launch/perception/stopbar_detector.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/rr_iarrc/src/stopbar_detector/CMakeLists.txt b/rr_iarrc/src/stopbar_detector/CMakeLists.txt new file mode 100644 index 000000000..0c3af85b8 --- /dev/null +++ b/rr_iarrc/src/stopbar_detector/CMakeLists.txt @@ -0,0 +1,2 @@ +add_executable(stopbar_detector stopbar_detector.cpp) +target_link_libraries(stopbar_detector ${catkin_LIBRARIES}) \ No newline at end of file diff --git a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp new file mode 100644 index 000000000..12d1f6eee --- /dev/null +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -0,0 +1,196 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +cv_bridge::CvImagePtr cv_ptrLine; +ros::Publisher pubLine; +ros::Publisher pubMove; + +rr_msgs::urc_sign moveMsg; + +double stopBarGoalAngle; +double stopBarGoalAngleRange; +double stopBarTriggerDistance; +double stopBarTriggerDistanceRight; +int houghThreshold; +double houghMinLineLength; +double houghMaxLineGap; +int pixels_per_meter; + +const std::string RIGHT("right"); +const std::string LEFT("left"); +const std::string STRAIGHT("straight"); +const std::string NONE("none"); + +struct ArrowSign { + std::string direction; + int area; + int count; +}; + +ArrowSign bestMove = { NONE, 0, 0 }; +/* + * Uses probablistic Hough to find line segments and determine if they are the + * stop bar An angle close to 0 is horizontal. + * + * @param frame The input overhead image to search inside + * @param output debug image + * @param stopBarGoalAngle The angle of line relative to horizontal that makes a + * stop bar + * @param stopBarGoalAngleRange Allowable error around stopBarGoalAngle + * @param triggerDistance Distance to the line that we will send out the message + * to take action + * @param threshold HoughLinesP threshold that determines # of votes that make a + * line + * @param minLineLength HoughLinesP minimum length of a line segment + * @param maxLineGap HoughLinesP maxmimum distance between points in the same + * line + */ +int findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, double stopBarGoalAngle, + double stopBarGoalAngleRange, double triggerDistance, double triggerDistanceRight, + int threshold, double minLineLength, double maxLineGap) { + cv::Mat edges; + int ddepth = CV_8UC1; + cv::Laplacian(frame, edges, ddepth); // use edge to get better Hough results + convertScaleAbs(edges, edges); + edges = frame; //#TOOD: probably laplace then dilate. + cv::cvtColor(edges, output, cv::COLOR_GRAY2BGR); // for debugging + + // Standard Hough Line Transform + std::vector lines; // will hold the results of the detection + double rho = 1; // distance resolution + double theta = CV_PI / 180; // angular resolution (in radians) pi/180 is one degree res + + cv::HoughLinesP(edges, lines, rho, theta, threshold, minLineLength, + maxLineGap); // Like hough but for line segments + for (size_t i = 0; i < lines.size(); i++) { + cv::Vec4i l = lines[i]; + cv::Point p1(l[0], l[1]); + cv::Point p2(l[2], l[3]); + cv::line(output, p1, p2, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); + + // calc angle and decide if it is a stop bar + double dx = p2.x - p1.x; + double dy = p2.y - p1.y; + double currAngle = atan(std::fabs(dy / dx)) * 180 / CV_PI; // in degrees + + cv::Point midpoint = (p1 + p2) * 0.5; + cv::circle(output, midpoint, 3, cv::Scalar(255, 0, 0), -1); + std::stringstream streamAngle; + streamAngle << std::fixed << std::setprecision(2) << currAngle; // show angle with a couple decimals + cv::putText(output, streamAngle.str(), midpoint, cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); + + if (fabs(stopBarGoalAngle - currAngle) <= stopBarGoalAngleRange) { // allows some amount of angle error + // get distance to the line + float dist = static_cast((edges.rows - midpoint.y)) / pixels_per_meter; + + cv::line(output, midpoint, cv::Point(midpoint.x, edges.rows), cv::Scalar(0, 255, 255), 1, cv::LINE_AA); + std::stringstream streamDist; + streamDist << std::fixed << std::setprecision(2) << dist; // show distance with a couple decimals + cv::putText(output, streamDist.str(), cv::Point(midpoint.x, edges.rows - dist / 2), cv::FONT_HERSHEY_PLAIN, + 1, cv::Scalar(0, 255, 0), 1); + + if (bestMove.direction == RIGHT && dist <= triggerDistanceRight) { + stopBarAngle = currAngle; + return 2; + } else if (dist <= triggerDistance) { + stopBarAngle = currAngle; + return 1; // stop bar detected close to us! + } + } + } + + return 0; // not close enough or no stop bar here +} + +void stopBar_callback(const sensor_msgs::ImageConstPtr& msg) { + cv_ptrLine = cv_bridge::toCvCopy(msg, "mono8"); + cv::Mat frame = cv_ptrLine->image; + cv::Mat debug; + double stopBarAngle; + int stopBarDetected = findStopBarFromHough(frame, debug, stopBarAngle, stopBarGoalAngle, stopBarGoalAngleRange, + stopBarTriggerDistance, stopBarTriggerDistanceRight, houghThreshold, + houghMinLineLength, houghMaxLineGap); + + // debugging draw a line where we trigger + cv::Point leftTriggerPoint(0, debug.rows - 1 - stopBarTriggerDistance * pixels_per_meter); + cv::Point rightTriggerPoint(debug.cols - 1, debug.rows - 1 - stopBarTriggerDistance * pixels_per_meter); + cv::line(debug, leftTriggerPoint, rightTriggerPoint, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); + + cv::Point leftTriggerPoint2(0, debug.rows - 1 - stopBarTriggerDistanceRight * pixels_per_meter); + cv::Point rightTriggerPoint2(debug.cols - 1, debug.rows - 1 - stopBarTriggerDistanceRight * pixels_per_meter); + cv::line(debug, leftTriggerPoint2, rightTriggerPoint2, cv::Scalar(0, 150, 0), 1, cv::LINE_AA); + + if (stopBarDetected == 2 && bestMove.direction != NONE) { + // let the world know + moveMsg.header.stamp = ros::Time::now(); + moveMsg.direction = bestMove.direction; + moveMsg.angle = stopBarAngle; + pubMove.publish(moveMsg); + + // reset things + bestMove.direction = NONE; + bestMove.area = 0.0; + } else if (stopBarDetected == 1 && bestMove.direction != NONE) { + // let the world know + moveMsg.header.stamp = ros::Time::now(); + moveMsg.direction = bestMove.direction; + moveMsg.angle = stopBarAngle; + pubMove.publish(moveMsg); + + // reset things + bestMove.direction = NONE; + bestMove.area = 0.0; + } else { + } + + if (pubLine.getNumSubscribers() > 0) { + sensor_msgs::Image outmsg; + cv_ptrLine->image = debug; + cv_ptrLine->encoding = "bgr8"; + cv_ptrLine->toImageMsg(outmsg); + pubLine.publish(outmsg); + } +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "sign_detector"); + + ros::NodeHandle nh; + ros::NodeHandle nhp("~"); + std::string overhead_image_sub; + std::string sign_file_path_from_package; + std::string sign_file_package_name; + + // stop bar detector params + nhp.param("overhead_image_subscription", overhead_image_sub, std::string("/lines/detection_img_transformed")); + nhp.param("stopBarGoalAngle", stopBarGoalAngle, 0.0); // angle in degrees + nhp.param("stopBarGoalAngleRange", stopBarGoalAngleRange, + 15.0); // angle in degrees + nhp.param("stopBarTriggerDistance", stopBarTriggerDistance, + 0.5); // distance in meters + nhp.param("stopBarTriggerDistanceRight", stopBarTriggerDistanceRight, + 0.5); // distance in meters + nhp.param("pixels_per_meter", pixels_per_meter, 100); + nhp.param("houghThreshold", houghThreshold, 50); + nhp.param("houghMinLineLength", houghMinLineLength, 0.0); + nhp.param("houghMaxLineGap", houghMaxLineGap, 0.0); + + pubLine = nh.advertise("/stopbar_detector/stop_bar", + 1); // debug publish of image + // publish the turn move for Urban Challenge Controller + pubMove = nh.advertise("/turn_detected", 1); + auto stopBar = nh.subscribe(overhead_image_sub, 1, stopBar_callback); + + ros::spin(); + return 0; +} \ No newline at end of file From 6a72c02387fa82453d9ce9c908dade32b2d71450 Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Mon, 8 Feb 2021 19:05:18 -0500 Subject: [PATCH 02/13] revises debug image to make it easier to read --- .../src/stopbar_detector/stopbar_detector.cpp | 49 ++++++------------- 1 file changed, 16 insertions(+), 33 deletions(-) diff --git a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp index 12d1f6eee..4d64e7045 100644 --- a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -37,7 +37,9 @@ struct ArrowSign { int count; }; +std::vector arrowTrackList{ { RIGHT, 0, 0 }, { LEFT, 0, 0 }, { STRAIGHT, 0, 0 } }; ArrowSign bestMove = { NONE, 0, 0 }; + /* * Uses probablistic Hough to find line segments and determine if they are the * stop bar An angle close to 0 is horizontal. @@ -55,7 +57,7 @@ ArrowSign bestMove = { NONE, 0, 0 }; * @param maxLineGap HoughLinesP maxmimum distance between points in the same * line */ -int findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, double stopBarGoalAngle, +bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, double stopBarGoalAngle, double stopBarGoalAngleRange, double triggerDistance, double triggerDistanceRight, int threshold, double minLineLength, double maxLineGap) { cv::Mat edges; @@ -79,32 +81,25 @@ int findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, cv::line(output, p1, p2, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); // calc angle and decide if it is a stop bar - double dx = p2.x - p1.x; - double dy = p2.y - p1.y; - double currAngle = atan(std::fabs(dy / dx)) * 180 / CV_PI; // in degrees + double distanceX = p2.x - p1.x; + double distanceY = p2.y - p1.y; + double currAngle = atan(fabs(distanceY / distanceX)) * 180 / CV_PI; // in degrees cv::Point midpoint = (p1 + p2) * 0.5; - cv::circle(output, midpoint, 3, cv::Scalar(255, 0, 0), -1); - std::stringstream streamAngle; - streamAngle << std::fixed << std::setprecision(2) << currAngle; // show angle with a couple decimals - cv::putText(output, streamAngle.str(), midpoint, cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); if (fabs(stopBarGoalAngle - currAngle) <= stopBarGoalAngleRange) { // allows some amount of angle error // get distance to the line - float dist = static_cast((edges.rows - midpoint.y)) / pixels_per_meter; + float dist = (edges.rows - midpoint.y) / pixels_per_meter; - cv::line(output, midpoint, cv::Point(midpoint.x, edges.rows), cv::Scalar(0, 255, 255), 1, cv::LINE_AA); - std::stringstream streamDist; - streamDist << std::fixed << std::setprecision(2) << dist; // show distance with a couple decimals - cv::putText(output, streamDist.str(), cv::Point(midpoint.x, edges.rows - dist / 2), cv::FONT_HERSHEY_PLAIN, - 1, cv::Scalar(0, 255, 0), 1); + if (dist <= triggerDistance) { + cv::Point midpoint = (p1 + p2) * 0.5; + cv::circle(output, midpoint, 3, cv::Scalar(255, 0, 0), -1); + std::stringstream streamAngle; + streamAngle << std::fixed << std::setprecision(2) << currAngle; // show angle with a couple decimals + cv::putText(output, streamAngle.str(), midpoint, cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); - if (bestMove.direction == RIGHT && dist <= triggerDistanceRight) { - stopBarAngle = currAngle; - return 2; - } else if (dist <= triggerDistance) { stopBarAngle = currAngle; - return 1; // stop bar detected close to us! + return true; // stop bar detected close to us! } } } @@ -117,7 +112,7 @@ void stopBar_callback(const sensor_msgs::ImageConstPtr& msg) { cv::Mat frame = cv_ptrLine->image; cv::Mat debug; double stopBarAngle; - int stopBarDetected = findStopBarFromHough(frame, debug, stopBarAngle, stopBarGoalAngle, stopBarGoalAngleRange, + bool stopBarDetected = findStopBarFromHough(frame, debug, stopBarAngle, stopBarGoalAngle, stopBarGoalAngleRange, stopBarTriggerDistance, stopBarTriggerDistanceRight, houghThreshold, houghMinLineLength, houghMaxLineGap); @@ -130,17 +125,7 @@ void stopBar_callback(const sensor_msgs::ImageConstPtr& msg) { cv::Point rightTriggerPoint2(debug.cols - 1, debug.rows - 1 - stopBarTriggerDistanceRight * pixels_per_meter); cv::line(debug, leftTriggerPoint2, rightTriggerPoint2, cv::Scalar(0, 150, 0), 1, cv::LINE_AA); - if (stopBarDetected == 2 && bestMove.direction != NONE) { - // let the world know - moveMsg.header.stamp = ros::Time::now(); - moveMsg.direction = bestMove.direction; - moveMsg.angle = stopBarAngle; - pubMove.publish(moveMsg); - - // reset things - bestMove.direction = NONE; - bestMove.area = 0.0; - } else if (stopBarDetected == 1 && bestMove.direction != NONE) { + if (stopBarDetected) { // let the world know moveMsg.header.stamp = ros::Time::now(); moveMsg.direction = bestMove.direction; @@ -150,9 +135,7 @@ void stopBar_callback(const sensor_msgs::ImageConstPtr& msg) { // reset things bestMove.direction = NONE; bestMove.area = 0.0; - } else { } - if (pubLine.getNumSubscribers() > 0) { sensor_msgs::Image outmsg; cv_ptrLine->image = debug; From e69cada8cc271894ad4a78bd95c74da18bc6751b Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Mon, 15 Feb 2021 18:28:51 -0500 Subject: [PATCH 03/13] revises debug image and what is published --- .../launch/perception/stopbar_detector.launch | 4 ++ .../src/stopbar_detector/stopbar_detector.cpp | 57 +++++++------------ 2 files changed, 25 insertions(+), 36 deletions(-) diff --git a/rr_iarrc/launch/perception/stopbar_detector.launch b/rr_iarrc/launch/perception/stopbar_detector.launch index a76d52889..16016ba47 100644 --- a/rr_iarrc/launch/perception/stopbar_detector.launch +++ b/rr_iarrc/launch/perception/stopbar_detector.launch @@ -1,5 +1,9 @@ + + + + diff --git a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp index 4d64e7045..87961a546 100644 --- a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -2,7 +2,8 @@ #include #include #include -#include +#include +#include #include #include @@ -12,10 +13,12 @@ #include cv_bridge::CvImagePtr cv_ptrLine; -ros::Publisher pubLine; -ros::Publisher pubMove; +ros::Publisher pub_line; +ros::Publisher pub_near_stopbar; +ros::Publisher pub_angle; -rr_msgs::urc_sign moveMsg; +std_msgs::Bool stop_bar_near; +std_msgs::Float64 stop_bar_angle; double stopBarGoalAngle; double stopBarGoalAngleRange; @@ -26,19 +29,6 @@ double houghMinLineLength; double houghMaxLineGap; int pixels_per_meter; -const std::string RIGHT("right"); -const std::string LEFT("left"); -const std::string STRAIGHT("straight"); -const std::string NONE("none"); - -struct ArrowSign { - std::string direction; - int area; - int count; -}; - -std::vector arrowTrackList{ { RIGHT, 0, 0 }, { LEFT, 0, 0 }, { STRAIGHT, 0, 0 } }; -ArrowSign bestMove = { NONE, 0, 0 }; /* * Uses probablistic Hough to find line segments and determine if they are the @@ -125,23 +115,17 @@ void stopBar_callback(const sensor_msgs::ImageConstPtr& msg) { cv::Point rightTriggerPoint2(debug.cols - 1, debug.rows - 1 - stopBarTriggerDistanceRight * pixels_per_meter); cv::line(debug, leftTriggerPoint2, rightTriggerPoint2, cv::Scalar(0, 150, 0), 1, cv::LINE_AA); - if (stopBarDetected) { - // let the world know - moveMsg.header.stamp = ros::Time::now(); - moveMsg.direction = bestMove.direction; - moveMsg.angle = stopBarAngle; - pubMove.publish(moveMsg); - - // reset things - bestMove.direction = NONE; - bestMove.area = 0.0; - } - if (pubLine.getNumSubscribers() > 0) { + stop_bar_near.data = stopBarDetected; + if (stopBarDetected) + stop_bar_angle.data = stopBarAngle; + pub_near_stopbar.publish(stop_bar_near); + pub_angle.publish(stop_bar_angle); + if (pub_line.getNumSubscribers() > 0) { sensor_msgs::Image outmsg; cv_ptrLine->image = debug; cv_ptrLine->encoding = "bgr8"; cv_ptrLine->toImageMsg(outmsg); - pubLine.publish(outmsg); + pub_line.publish(outmsg); } } @@ -151,10 +135,11 @@ int main(int argc, char** argv) { ros::NodeHandle nh; ros::NodeHandle nhp("~"); std::string overhead_image_sub; - std::string sign_file_path_from_package; - std::string sign_file_package_name; + std::string stopbar_near_topic; + std::string stopbar_angle_topic; + nhp.param("stopbar_near", stopbar_near_topic, std::string("/stopbar_near")); + nhp.param("stopbar_angle", stopbar_angle_topic, std::string("/stopbar_angle")); - // stop bar detector params nhp.param("overhead_image_subscription", overhead_image_sub, std::string("/lines/detection_img_transformed")); nhp.param("stopBarGoalAngle", stopBarGoalAngle, 0.0); // angle in degrees nhp.param("stopBarGoalAngleRange", stopBarGoalAngleRange, @@ -168,10 +153,10 @@ int main(int argc, char** argv) { nhp.param("houghMinLineLength", houghMinLineLength, 0.0); nhp.param("houghMaxLineGap", houghMaxLineGap, 0.0); - pubLine = nh.advertise("/stopbar_detector/stop_bar", + pub_line = nh.advertise("/stopbar_detector/stop_bar", 1); // debug publish of image - // publish the turn move for Urban Challenge Controller - pubMove = nh.advertise("/turn_detected", 1); + pub_near_stopbar = nhp.advertise(stopbar_near_topic, 1); + pub_angle = nhp.advertise(stopbar_angle_topic, 1); auto stopBar = nh.subscribe(overhead_image_sub, 1, stopBar_callback); ros::spin(); From 8e99802578f4ae13158e6bddc8b30a611cf2aeba Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Mon, 8 Mar 2021 18:27:49 -0500 Subject: [PATCH 04/13] adds a delay to sending when the stopbar is near so the robot is at the stopbar --- .../launch/perception/stopbar_detector.launch | 9 +-- .../src/stopbar_detector/stopbar_detector.cpp | 55 ++++++++++++++----- 2 files changed, 45 insertions(+), 19 deletions(-) diff --git a/rr_iarrc/launch/perception/stopbar_detector.launch b/rr_iarrc/launch/perception/stopbar_detector.launch index 16016ba47..412faba6e 100644 --- a/rr_iarrc/launch/perception/stopbar_detector.launch +++ b/rr_iarrc/launch/perception/stopbar_detector.launch @@ -1,6 +1,7 @@ - + + @@ -8,12 +9,12 @@ - - - + + + diff --git a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp index 87961a546..f55ac90c7 100644 --- a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -20,15 +21,22 @@ ros::Publisher pub_angle; std_msgs::Bool stop_bar_near; std_msgs::Float64 stop_bar_angle; + + double stopBarGoalAngle; double stopBarGoalAngleRange; double stopBarTriggerDistance; -double stopBarTriggerDistanceRight; int houghThreshold; double houghMinLineLength; double houghMaxLineGap; int pixels_per_meter; +double speed; +double sleepConstant; + +cv::Mat kernel(int x, int y) { + return cv::getStructuringElement(cv::MORPH_RECT, cv::Size(x, y)); +} /* * Uses probablistic Hough to find line segments and determine if they are the @@ -48,13 +56,14 @@ int pixels_per_meter; * line */ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, double stopBarGoalAngle, - double stopBarGoalAngleRange, double triggerDistance, double triggerDistanceRight, + double stopBarGoalAngleRange, double triggerDistance, int threshold, double minLineLength, double maxLineGap) { cv::Mat edges; int ddepth = CV_8UC1; cv::Laplacian(frame, edges, ddepth); // use edge to get better Hough results convertScaleAbs(edges, edges); - edges = frame; //#TOOD: probably laplace then dilate. + edges = frame; + cv::dilate(edges, edges, kernel(4, 4)); cv::cvtColor(edges, output, cv::COLOR_GRAY2BGR); // for debugging // Standard Hough Line Transform @@ -79,22 +88,31 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, if (fabs(stopBarGoalAngle - currAngle) <= stopBarGoalAngleRange) { // allows some amount of angle error // get distance to the line - float dist = (edges.rows - midpoint.y) / pixels_per_meter; + float dist = static_cast((edges.rows - midpoint.y)) / pixels_per_meter; if (dist <= triggerDistance) { - cv::Point midpoint = (p1 + p2) * 0.5; + // places circle in the center of the line and displays angle of line in debug image cv::circle(output, midpoint, 3, cv::Scalar(255, 0, 0), -1); std::stringstream streamAngle; streamAngle << std::fixed << std::setprecision(2) << currAngle; // show angle with a couple decimals cv::putText(output, streamAngle.str(), midpoint, cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); + // draw line to stopbar in debug image and displays the distance in meters to it + cv::line(output, midpoint, cv::Point(midpoint.x, edges.rows), cv::Scalar(0, 255, 255), 1, cv::LINE_AA); + std::stringstream streamDist; + streamDist << std::fixed << std::setprecision(2) << dist; // show distance with a couple decimals + cv::putText(output, streamDist.str(), cv::Point(midpoint.x, edges.rows - dist / 2), cv::FONT_HERSHEY_PLAIN, + 1, cv::Scalar(0, 255, 0), 1); + stopBarAngle = currAngle; + double timeToStopBar = dist / speed - sleepConstant; + ros::Duration(timeToStopBar).sleep(); return true; // stop bar detected close to us! } } } - return 0; // not close enough or no stop bar here + return false; // not close enough or no stop bar here } void stopBar_callback(const sensor_msgs::ImageConstPtr& msg) { @@ -103,17 +121,14 @@ void stopBar_callback(const sensor_msgs::ImageConstPtr& msg) { cv::Mat debug; double stopBarAngle; bool stopBarDetected = findStopBarFromHough(frame, debug, stopBarAngle, stopBarGoalAngle, stopBarGoalAngleRange, - stopBarTriggerDistance, stopBarTriggerDistanceRight, houghThreshold, + stopBarTriggerDistance, houghThreshold, houghMinLineLength, houghMaxLineGap); // debugging draw a line where we trigger - cv::Point leftTriggerPoint(0, debug.rows - 1 - stopBarTriggerDistance * pixels_per_meter); - cv::Point rightTriggerPoint(debug.cols - 1, debug.rows - 1 - stopBarTriggerDistance * pixels_per_meter); - cv::line(debug, leftTriggerPoint, rightTriggerPoint, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); + cv::Point leftPoint(0, debug.rows - 1 - stopBarTriggerDistance * pixels_per_meter); + cv::Point rightPoint(debug.cols - 1, debug.rows - 1 - stopBarTriggerDistance * pixels_per_meter); + cv::line(debug, leftPoint, rightPoint, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); - cv::Point leftTriggerPoint2(0, debug.rows - 1 - stopBarTriggerDistanceRight * pixels_per_meter); - cv::Point rightTriggerPoint2(debug.cols - 1, debug.rows - 1 - stopBarTriggerDistanceRight * pixels_per_meter); - cv::line(debug, leftTriggerPoint2, rightTriggerPoint2, cv::Scalar(0, 150, 0), 1, cv::LINE_AA); stop_bar_near.data = stopBarDetected; if (stopBarDetected) @@ -129,25 +144,34 @@ void stopBar_callback(const sensor_msgs::ImageConstPtr& msg) { } } +void speed_callback(const rr_msgs::speed& speedMsg) { + speed = speedMsg.speed; +} + int main(int argc, char** argv) { ros::init(argc, argv, "sign_detector"); ros::NodeHandle nh; ros::NodeHandle nhp("~"); + std::string overhead_image_sub; std::string stopbar_near_topic; std::string stopbar_angle_topic; + std::string speed_topic; + nhp.param("stopbar_near", stopbar_near_topic, std::string("/stopbar_near")); nhp.param("stopbar_angle", stopbar_angle_topic, std::string("/stopbar_angle")); + nhp.param("sleep_adjust", sleepConstant, 1.0); nhp.param("overhead_image_subscription", overhead_image_sub, std::string("/lines/detection_img_transformed")); + + nhp.param("speed_subscription", speed_topic, std::string("/speed")); + nhp.param("stopBarGoalAngle", stopBarGoalAngle, 0.0); // angle in degrees nhp.param("stopBarGoalAngleRange", stopBarGoalAngleRange, 15.0); // angle in degrees nhp.param("stopBarTriggerDistance", stopBarTriggerDistance, 0.5); // distance in meters - nhp.param("stopBarTriggerDistanceRight", stopBarTriggerDistanceRight, - 0.5); // distance in meters nhp.param("pixels_per_meter", pixels_per_meter, 100); nhp.param("houghThreshold", houghThreshold, 50); nhp.param("houghMinLineLength", houghMinLineLength, 0.0); @@ -157,6 +181,7 @@ int main(int argc, char** argv) { 1); // debug publish of image pub_near_stopbar = nhp.advertise(stopbar_near_topic, 1); pub_angle = nhp.advertise(stopbar_angle_topic, 1); + auto speed_sub = nhp.subscribe(speed_topic, 1, speed_callback); auto stopBar = nh.subscribe(overhead_image_sub, 1, stopBar_callback); ros::spin(); From 02874179fce63f230c6bdb37b7bf0d85807ebc54 Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Mon, 8 Mar 2021 18:34:41 -0500 Subject: [PATCH 05/13] fixes formatting --- .../src/stopbar_detector/stopbar_detector.cpp | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp index f55ac90c7..06233c9c2 100644 --- a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -3,9 +3,9 @@ #include #include #include +#include #include #include -#include #include #include @@ -21,8 +21,6 @@ ros::Publisher pub_angle; std_msgs::Bool stop_bar_near; std_msgs::Float64 stop_bar_angle; - - double stopBarGoalAngle; double stopBarGoalAngleRange; double stopBarTriggerDistance; @@ -56,8 +54,8 @@ cv::Mat kernel(int x, int y) { * line */ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, double stopBarGoalAngle, - double stopBarGoalAngleRange, double triggerDistance, - int threshold, double minLineLength, double maxLineGap) { + double stopBarGoalAngleRange, double triggerDistance, int threshold, double minLineLength, + double maxLineGap) { cv::Mat edges; int ddepth = CV_8UC1; cv::Laplacian(frame, edges, ddepth); // use edge to get better Hough results @@ -88,7 +86,7 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, if (fabs(stopBarGoalAngle - currAngle) <= stopBarGoalAngleRange) { // allows some amount of angle error // get distance to the line - float dist = static_cast((edges.rows - midpoint.y)) / pixels_per_meter; + float dist = static_cast((edges.rows - midpoint.y)) / pixels_per_meter; if (dist <= triggerDistance) { // places circle in the center of the line and displays angle of line in debug image @@ -101,8 +99,8 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, cv::line(output, midpoint, cv::Point(midpoint.x, edges.rows), cv::Scalar(0, 255, 255), 1, cv::LINE_AA); std::stringstream streamDist; streamDist << std::fixed << std::setprecision(2) << dist; // show distance with a couple decimals - cv::putText(output, streamDist.str(), cv::Point(midpoint.x, edges.rows - dist / 2), cv::FONT_HERSHEY_PLAIN, - 1, cv::Scalar(0, 255, 0), 1); + cv::putText(output, streamDist.str(), cv::Point(midpoint.x, edges.rows - dist / 2), + cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); stopBarAngle = currAngle; double timeToStopBar = dist / speed - sleepConstant; @@ -120,16 +118,15 @@ void stopBar_callback(const sensor_msgs::ImageConstPtr& msg) { cv::Mat frame = cv_ptrLine->image; cv::Mat debug; double stopBarAngle; - bool stopBarDetected = findStopBarFromHough(frame, debug, stopBarAngle, stopBarGoalAngle, stopBarGoalAngleRange, - stopBarTriggerDistance, houghThreshold, - houghMinLineLength, houghMaxLineGap); + bool stopBarDetected = + findStopBarFromHough(frame, debug, stopBarAngle, stopBarGoalAngle, stopBarGoalAngleRange, + stopBarTriggerDistance, houghThreshold, houghMinLineLength, houghMaxLineGap); // debugging draw a line where we trigger cv::Point leftPoint(0, debug.rows - 1 - stopBarTriggerDistance * pixels_per_meter); cv::Point rightPoint(debug.cols - 1, debug.rows - 1 - stopBarTriggerDistance * pixels_per_meter); cv::line(debug, leftPoint, rightPoint, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); - stop_bar_near.data = stopBarDetected; if (stopBarDetected) stop_bar_angle.data = stopBarAngle; @@ -178,7 +175,7 @@ int main(int argc, char** argv) { nhp.param("houghMaxLineGap", houghMaxLineGap, 0.0); pub_line = nh.advertise("/stopbar_detector/stop_bar", - 1); // debug publish of image + 1); // debug publish of image pub_near_stopbar = nhp.advertise(stopbar_near_topic, 1); pub_angle = nhp.advertise(stopbar_angle_topic, 1); auto speed_sub = nhp.subscribe(speed_topic, 1, speed_callback); From a6e4d1513022a1477dfd6528608b5ee21ffb4694 Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Mon, 8 Mar 2021 19:05:24 -0500 Subject: [PATCH 06/13] adjusts time that sleep is modified by --- rr_iarrc/launch/perception/stopbar_detector.launch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rr_iarrc/launch/perception/stopbar_detector.launch b/rr_iarrc/launch/perception/stopbar_detector.launch index 412faba6e..a55bc8fb6 100644 --- a/rr_iarrc/launch/perception/stopbar_detector.launch +++ b/rr_iarrc/launch/perception/stopbar_detector.launch @@ -9,12 +9,12 @@ - - + + - + From 033268d3edb039219f49861de77d82f4338dde04 Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Sun, 14 Mar 2021 14:50:16 -0400 Subject: [PATCH 07/13] corrects sleep time for stopbar detector --- rr_iarrc/launch/perception/stopbar_detector.launch | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rr_iarrc/launch/perception/stopbar_detector.launch b/rr_iarrc/launch/perception/stopbar_detector.launch index a55bc8fb6..f3e895efc 100644 --- a/rr_iarrc/launch/perception/stopbar_detector.launch +++ b/rr_iarrc/launch/perception/stopbar_detector.launch @@ -1,7 +1,6 @@ - @@ -14,7 +13,7 @@ - + From 8d800edf55e148b20cda54f9dc0e448756f11d4b Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Sun, 14 Mar 2021 17:21:26 -0400 Subject: [PATCH 08/13] prevents division by zero when speed is zero --- rr_iarrc/src/stopbar_detector/stopbar_detector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp index 06233c9c2..699e34d6c 100644 --- a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -88,7 +88,7 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, // get distance to the line float dist = static_cast((edges.rows - midpoint.y)) / pixels_per_meter; - if (dist <= triggerDistance) { + if (dist <= triggerDistance && speed != 0) { // places circle in the center of the line and displays angle of line in debug image cv::circle(output, midpoint, 3, cv::Scalar(255, 0, 0), -1); std::stringstream streamAngle; From 7ebf5034f1f05d0df0df555b5072ab1d63b77a89 Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Mon, 15 Mar 2021 19:31:27 -0400 Subject: [PATCH 09/13] multiples sleep time by a constant instead of subtracting --- .../launch/perception/stopbar_detector.launch | 15 ++++++++----- .../src/stopbar_detector/stopbar_detector.cpp | 22 +++++++++---------- 2 files changed, 20 insertions(+), 17 deletions(-) diff --git a/rr_iarrc/launch/perception/stopbar_detector.launch b/rr_iarrc/launch/perception/stopbar_detector.launch index f3e895efc..bd83a4a36 100644 --- a/rr_iarrc/launch/perception/stopbar_detector.launch +++ b/rr_iarrc/launch/perception/stopbar_detector.launch @@ -1,19 +1,22 @@ - + - + + + + - - + + + + - - diff --git a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp index 699e34d6c..8e409c663 100644 --- a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -28,9 +28,9 @@ int houghThreshold; double houghMinLineLength; double houghMaxLineGap; int pixels_per_meter; +double sleepConstant; double speed; -double sleepConstant; cv::Mat kernel(int x, int y) { return cv::getStructuringElement(cv::MORPH_RECT, cv::Size(x, y)); @@ -61,7 +61,7 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, cv::Laplacian(frame, edges, ddepth); // use edge to get better Hough results convertScaleAbs(edges, edges); edges = frame; - cv::dilate(edges, edges, kernel(4, 4)); + cv::dilate(edges, edges, kernel(4, 4)); // clearer debug image and slightly better detection cv::cvtColor(edges, output, cv::COLOR_GRAY2BGR); // for debugging // Standard Hough Line Transform @@ -86,9 +86,9 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, if (fabs(stopBarGoalAngle - currAngle) <= stopBarGoalAngleRange) { // allows some amount of angle error // get distance to the line - float dist = static_cast((edges.rows - midpoint.y)) / pixels_per_meter; + float dist = static_cast(edges.rows - midpoint.y) / pixels_per_meter; - if (dist <= triggerDistance && speed != 0) { + if (dist <= triggerDistance) { // places circle in the center of the line and displays angle of line in debug image cv::circle(output, midpoint, 3, cv::Scalar(255, 0, 0), -1); std::stringstream streamAngle; @@ -103,8 +103,10 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); stopBarAngle = currAngle; - double timeToStopBar = dist / speed - sleepConstant; - ros::Duration(timeToStopBar).sleep(); + if (speed != 0) { + double timeToStopBar = (dist / speed) * sleepConstant; + ros::Duration(timeToStopBar).sleep(); + } return true; // stop bar detected close to us! } } @@ -164,11 +166,9 @@ int main(int argc, char** argv) { nhp.param("speed_subscription", speed_topic, std::string("/speed")); - nhp.param("stopBarGoalAngle", stopBarGoalAngle, 0.0); // angle in degrees - nhp.param("stopBarGoalAngleRange", stopBarGoalAngleRange, - 15.0); // angle in degrees - nhp.param("stopBarTriggerDistance", stopBarTriggerDistance, - 0.5); // distance in meters + nhp.param("stopBarGoalAngle", stopBarGoalAngle, 0.0); // angle in degrees + nhp.param("stopBarGoalAngleRange", stopBarGoalAngleRange, 15.0); // angle in degrees + nhp.param("stopBarTriggerDistance", stopBarTriggerDistance, 0.5); // distance in meters nhp.param("pixels_per_meter", pixels_per_meter, 100); nhp.param("houghThreshold", houghThreshold, 50); nhp.param("houghMinLineLength", houghMinLineLength, 0.0); From fed536299de3b2bfb34081482b566b410812f696 Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Mon, 29 Mar 2021 17:56:56 -0400 Subject: [PATCH 10/13] Improves code readability --- .../src/stopbar_detector/stopbar_detector.cpp | 45 ++++++++----------- 1 file changed, 19 insertions(+), 26 deletions(-) diff --git a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp index 8e409c663..4224c056b 100644 --- a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -30,6 +30,9 @@ double houghMaxLineGap; int pixels_per_meter; double sleepConstant; +const double rho = 1; // distance resolution +const double theta = CV_PI / 180; // angular resolution (in radians) pi/180 is one degree res + double speed; cv::Mat kernel(int x, int y) { @@ -53,53 +56,45 @@ cv::Mat kernel(int x, int y) { * @param maxLineGap HoughLinesP maxmimum distance between points in the same * line */ -bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, double stopBarGoalAngle, - double stopBarGoalAngleRange, double triggerDistance, int threshold, double minLineLength, - double maxLineGap) { +bool findStopBarFromHough(cv::Mat& frame, cv::Mat& debug, double& stopBarAngle) { cv::Mat edges; int ddepth = CV_8UC1; cv::Laplacian(frame, edges, ddepth); // use edge to get better Hough results - convertScaleAbs(edges, edges); - edges = frame; - cv::dilate(edges, edges, kernel(4, 4)); // clearer debug image and slightly better detection - cv::cvtColor(edges, output, cv::COLOR_GRAY2BGR); // for debugging + cv::dilate(frame, frame, kernel(4, 4)); // clearer debug image and slightly better detection + cv::cvtColor(frame, debug, cv::COLOR_GRAY2BGR); // for debugging // Standard Hough Line Transform std::vector lines; // will hold the results of the detection - double rho = 1; // distance resolution - double theta = CV_PI / 180; // angular resolution (in radians) pi/180 is one degree res - cv::HoughLinesP(edges, lines, rho, theta, threshold, minLineLength, - maxLineGap); // Like hough but for line segments + cv::HoughLinesP(frame, lines, rho, theta, houghThreshold, houghMinLineLength, houghMaxLineGap); // Like hough but for line segments for (size_t i = 0; i < lines.size(); i++) { cv::Vec4i l = lines[i]; cv::Point p1(l[0], l[1]); cv::Point p2(l[2], l[3]); - cv::line(output, p1, p2, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); + cv::line(debug, p1, p2, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); // calc angle and decide if it is a stop bar - double distanceX = p2.x - p1.x; + double distanceX = 0; double distanceY = p2.y - p1.y; - double currAngle = atan(fabs(distanceY / distanceX)) * 180 / CV_PI; // in degrees - + double currAngle = fabs(atan(distanceY / distanceX)) * 180 / CV_PI; // in degrees/ cv::Point midpoint = (p1 + p2) * 0.5; if (fabs(stopBarGoalAngle - currAngle) <= stopBarGoalAngleRange) { // allows some amount of angle error // get distance to the line float dist = static_cast(edges.rows - midpoint.y) / pixels_per_meter; - if (dist <= triggerDistance) { + if (dist <= stopBarTriggerDistance) { // places circle in the center of the line and displays angle of line in debug image - cv::circle(output, midpoint, 3, cv::Scalar(255, 0, 0), -1); + cv::circle(debug, midpoint, 3, cv::Scalar(255, 0, 0), -1); std::stringstream streamAngle; streamAngle << std::fixed << std::setprecision(2) << currAngle; // show angle with a couple decimals - cv::putText(output, streamAngle.str(), midpoint, cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); + cv::putText(debug, streamAngle.str(), midpoint, cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); // draw line to stopbar in debug image and displays the distance in meters to it - cv::line(output, midpoint, cv::Point(midpoint.x, edges.rows), cv::Scalar(0, 255, 255), 1, cv::LINE_AA); + cv::line(debug, midpoint, cv::Point(midpoint.x, edges.rows), cv::Scalar(0, 255, 255), 1, cv::LINE_AA); std::stringstream streamDist; streamDist << std::fixed << std::setprecision(2) << dist; // show distance with a couple decimals - cv::putText(output, streamDist.str(), cv::Point(midpoint.x, edges.rows - dist / 2), + cv::putText(debug, streamDist.str(), cv::Point(midpoint.x, edges.rows - dist / 2), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); stopBarAngle = currAngle; @@ -111,7 +106,6 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& output, double& stopBarAngle, } } } - return false; // not close enough or no stop bar here } @@ -120,9 +114,7 @@ void stopBar_callback(const sensor_msgs::ImageConstPtr& msg) { cv::Mat frame = cv_ptrLine->image; cv::Mat debug; double stopBarAngle; - bool stopBarDetected = - findStopBarFromHough(frame, debug, stopBarAngle, stopBarGoalAngle, stopBarGoalAngleRange, - stopBarTriggerDistance, houghThreshold, houghMinLineLength, houghMaxLineGap); + bool stopBarDetected = findStopBarFromHough(frame, debug, stopBarAngle); // debugging draw a line where we trigger cv::Point leftPoint(0, debug.rows - 1 - stopBarTriggerDistance * pixels_per_meter); @@ -130,10 +122,11 @@ void stopBar_callback(const sensor_msgs::ImageConstPtr& msg) { cv::line(debug, leftPoint, rightPoint, cv::Scalar(0, 255, 0), 1, cv::LINE_AA); stop_bar_near.data = stopBarDetected; - if (stopBarDetected) + if (stopBarDetected) { stop_bar_angle.data = stopBarAngle; + pub_angle.publish(stop_bar_angle); + } pub_near_stopbar.publish(stop_bar_near); - pub_angle.publish(stop_bar_angle); if (pub_line.getNumSubscribers() > 0) { sensor_msgs::Image outmsg; cv_ptrLine->image = debug; From 1d0c6a5320ac29e7acdc121d2ab1d0c88872cd18 Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Mon, 29 Mar 2021 18:57:18 -0400 Subject: [PATCH 11/13] improves handling of angles --- .../src/stopbar_detector/stopbar_detector.cpp | 24 +++++++------------ 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp index 4224c056b..df4725251 100644 --- a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -44,17 +45,7 @@ cv::Mat kernel(int x, int y) { * stop bar An angle close to 0 is horizontal. * * @param frame The input overhead image to search inside - * @param output debug image - * @param stopBarGoalAngle The angle of line relative to horizontal that makes a - * stop bar - * @param stopBarGoalAngleRange Allowable error around stopBarGoalAngle - * @param triggerDistance Distance to the line that we will send out the message - * to take action - * @param threshold HoughLinesP threshold that determines # of votes that make a - * line - * @param minLineLength HoughLinesP minimum length of a line segment - * @param maxLineGap HoughLinesP maxmimum distance between points in the same - * line + * @param debug The debug image */ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& debug, double& stopBarAngle) { cv::Mat edges; @@ -74,12 +65,13 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& debug, double& stopBarAngle) cv::line(debug, p1, p2, cv::Scalar(0, 0, 255), 2, cv::LINE_AA); // calc angle and decide if it is a stop bar - double distanceX = 0; + double distanceX = p2.x - p1.x; double distanceY = p2.y - p1.y; - double currAngle = fabs(atan(distanceY / distanceX)) * 180 / CV_PI; // in degrees/ + double currAngle = fabs(atan(distanceY / distanceX)); // in radians cv::Point midpoint = (p1 + p2) * 0.5; - - if (fabs(stopBarGoalAngle - currAngle) <= stopBarGoalAngleRange) { // allows some amount of angle error + double goalAngleRad = stopBarGoalAngle * CV_PI/180; + double angleDiff = fabs(angles::shortest_angular_distance(stopBarGoalAngle, currAngle) * 180/CV_PI); //in degrees + if (angleDiff <= stopBarGoalAngleRange) { // allows some amount of angle error // get distance to the line float dist = static_cast(edges.rows - midpoint.y) / pixels_per_meter; @@ -87,7 +79,7 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& debug, double& stopBarAngle) // places circle in the center of the line and displays angle of line in debug image cv::circle(debug, midpoint, 3, cv::Scalar(255, 0, 0), -1); std::stringstream streamAngle; - streamAngle << std::fixed << std::setprecision(2) << currAngle; // show angle with a couple decimals + streamAngle << std::fixed << std::setprecision(2) << (currAngle * 180/CV_PI); // show angle with a couple decimals cv::putText(debug, streamAngle.str(), midpoint, cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); // draw line to stopbar in debug image and displays the distance in meters to it From 29b6d6b4364ecb04f17ae7c79ae11dde8524cc07 Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Sun, 4 Apr 2021 13:22:46 -0400 Subject: [PATCH 12/13] improve format and style --- .../src/stopbar_detector/stopbar_detector.cpp | 33 ++++++++++--------- 1 file changed, 18 insertions(+), 15 deletions(-) diff --git a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp index df4725251..8589c81b1 100644 --- a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -1,8 +1,8 @@ +#include #include #include #include #include -#include #include #include #include @@ -31,8 +31,8 @@ double houghMaxLineGap; int pixels_per_meter; double sleepConstant; -const double rho = 1; // distance resolution -const double theta = CV_PI / 180; // angular resolution (in radians) pi/180 is one degree res +const double rho = 1; // distance resolution +const double theta = CV_PI / 180; // angular resolution (in radians) pi/180 is one degree res double speed; @@ -50,14 +50,15 @@ cv::Mat kernel(int x, int y) { bool findStopBarFromHough(cv::Mat& frame, cv::Mat& debug, double& stopBarAngle) { cv::Mat edges; int ddepth = CV_8UC1; - cv::Laplacian(frame, edges, ddepth); // use edge to get better Hough results - cv::dilate(frame, frame, kernel(4, 4)); // clearer debug image and slightly better detection + cv::Laplacian(frame, edges, ddepth); // use edge to get better Hough results + cv::dilate(frame, frame, kernel(4, 4)); // clearer debug image and slightly better detection cv::cvtColor(frame, debug, cv::COLOR_GRAY2BGR); // for debugging // Standard Hough Line Transform std::vector lines; // will hold the results of the detection - cv::HoughLinesP(frame, lines, rho, theta, houghThreshold, houghMinLineLength, houghMaxLineGap); // Like hough but for line segments + cv::HoughLinesP(frame, lines, rho, theta, houghThreshold, houghMinLineLength, + houghMaxLineGap); // Like hough but for line segments for (size_t i = 0; i < lines.size(); i++) { cv::Vec4i l = lines[i]; cv::Point p1(l[0], l[1]); @@ -69,8 +70,9 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& debug, double& stopBarAngle) double distanceY = p2.y - p1.y; double currAngle = fabs(atan(distanceY / distanceX)); // in radians cv::Point midpoint = (p1 + p2) * 0.5; - double goalAngleRad = stopBarGoalAngle * CV_PI/180; - double angleDiff = fabs(angles::shortest_angular_distance(stopBarGoalAngle, currAngle) * 180/CV_PI); //in degrees + double goalAngleRad = stopBarGoalAngle * CV_PI / 180; + double angleDiff = + fabs(angles::shortest_angular_distance(stopBarGoalAngle, currAngle) * 180 / CV_PI); // in degrees if (angleDiff <= stopBarGoalAngleRange) { // allows some amount of angle error // get distance to the line float dist = static_cast(edges.rows - midpoint.y) / pixels_per_meter; @@ -79,7 +81,8 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& debug, double& stopBarAngle) // places circle in the center of the line and displays angle of line in debug image cv::circle(debug, midpoint, 3, cv::Scalar(255, 0, 0), -1); std::stringstream streamAngle; - streamAngle << std::fixed << std::setprecision(2) << (currAngle * 180/CV_PI); // show angle with a couple decimals + streamAngle << std::fixed << std::setprecision(2) + << (currAngle * 180 / CV_PI); // show angle with a couple decimals cv::putText(debug, streamAngle.str(), midpoint, cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); // draw line to stopbar in debug image and displays the distance in meters to it @@ -143,13 +146,13 @@ int main(int argc, char** argv) { std::string stopbar_angle_topic; std::string speed_topic; - nhp.param("stopbar_near", stopbar_near_topic, std::string("/stopbar_near")); - nhp.param("stopbar_angle", stopbar_angle_topic, std::string("/stopbar_angle")); + nh.param("stopbar_near", stopbar_near_topic, std::string("/stopbar_near")); + nh.param("stopbar_angle", stopbar_angle_topic, std::string("/stopbar_angle")); nhp.param("sleep_adjust", sleepConstant, 1.0); nhp.param("overhead_image_subscription", overhead_image_sub, std::string("/lines/detection_img_transformed")); - nhp.param("speed_subscription", speed_topic, std::string("/speed")); + nh.param("speed_subscription", speed_topic, std::string("/speed")); nhp.param("stopBarGoalAngle", stopBarGoalAngle, 0.0); // angle in degrees nhp.param("stopBarGoalAngleRange", stopBarGoalAngleRange, 15.0); // angle in degrees @@ -161,9 +164,9 @@ int main(int argc, char** argv) { pub_line = nh.advertise("/stopbar_detector/stop_bar", 1); // debug publish of image - pub_near_stopbar = nhp.advertise(stopbar_near_topic, 1); - pub_angle = nhp.advertise(stopbar_angle_topic, 1); - auto speed_sub = nhp.subscribe(speed_topic, 1, speed_callback); + pub_near_stopbar = nh.advertise(stopbar_near_topic, 1); + pub_angle = nh.advertise(stopbar_angle_topic, 1); + auto speed_sub = nh.subscribe(speed_topic, 1, speed_callback); auto stopBar = nh.subscribe(overhead_image_sub, 1, stopBar_callback); ros::spin(); From 4c828fac3e2e75b99294df1af3253c7ee8555ed7 Mon Sep 17 00:00:00 2001 From: Isaac Dale Date: Sun, 4 Apr 2021 17:11:07 -0400 Subject: [PATCH 13/13] makes all angles radians and removes unused laplace --- rr_iarrc/src/stopbar_detector/stopbar_detector.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp index 8589c81b1..43e8067a0 100644 --- a/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -48,9 +48,7 @@ cv::Mat kernel(int x, int y) { * @param debug The debug image */ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& debug, double& stopBarAngle) { - cv::Mat edges; int ddepth = CV_8UC1; - cv::Laplacian(frame, edges, ddepth); // use edge to get better Hough results cv::dilate(frame, frame, kernel(4, 4)); // clearer debug image and slightly better detection cv::cvtColor(frame, debug, cv::COLOR_GRAY2BGR); // for debugging @@ -70,12 +68,10 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& debug, double& stopBarAngle) double distanceY = p2.y - p1.y; double currAngle = fabs(atan(distanceY / distanceX)); // in radians cv::Point midpoint = (p1 + p2) * 0.5; - double goalAngleRad = stopBarGoalAngle * CV_PI / 180; - double angleDiff = - fabs(angles::shortest_angular_distance(stopBarGoalAngle, currAngle) * 180 / CV_PI); // in degrees + double angleDiff = fabs(angles::shortest_angular_distance(stopBarAngle, currAngle)); if (angleDiff <= stopBarGoalAngleRange) { // allows some amount of angle error // get distance to the line - float dist = static_cast(edges.rows - midpoint.y) / pixels_per_meter; + float dist = static_cast(frame.rows - midpoint.y) / pixels_per_meter; if (dist <= stopBarTriggerDistance) { // places circle in the center of the line and displays angle of line in debug image @@ -86,10 +82,10 @@ bool findStopBarFromHough(cv::Mat& frame, cv::Mat& debug, double& stopBarAngle) cv::putText(debug, streamAngle.str(), midpoint, cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); // draw line to stopbar in debug image and displays the distance in meters to it - cv::line(debug, midpoint, cv::Point(midpoint.x, edges.rows), cv::Scalar(0, 255, 255), 1, cv::LINE_AA); + cv::line(debug, midpoint, cv::Point(midpoint.x, frame.rows), cv::Scalar(0, 255, 255), 1, cv::LINE_AA); std::stringstream streamDist; streamDist << std::fixed << std::setprecision(2) << dist; // show distance with a couple decimals - cv::putText(debug, streamDist.str(), cv::Point(midpoint.x, edges.rows - dist / 2), + cv::putText(debug, streamDist.str(), cv::Point(midpoint.x, frame.rows - dist / 2), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); stopBarAngle = currAngle; @@ -156,6 +152,8 @@ int main(int argc, char** argv) { nhp.param("stopBarGoalAngle", stopBarGoalAngle, 0.0); // angle in degrees nhp.param("stopBarGoalAngleRange", stopBarGoalAngleRange, 15.0); // angle in degrees + stopBarGoalAngle *= CV_PI / 180; // convert to radians + stopBarGoalAngleRange *= CV_PI / 180; // convert to radians nhp.param("stopBarTriggerDistance", stopBarTriggerDistance, 0.5); // distance in meters nhp.param("pixels_per_meter", pixels_per_meter, 100); nhp.param("houghThreshold", houghThreshold, 50);