diff --git a/rr_iarrc/CMakeLists.txt b/rr_iarrc/CMakeLists.txt index aebfbd6f2..47804c5f4 100644 --- a/rr_iarrc/CMakeLists.txt +++ b/rr_iarrc/CMakeLists.txt @@ -39,5 +39,6 @@ add_subdirectory(src/drag_centerline_planner) add_subdirectory(src/cone_detection) add_subdirectory(src/urc_controller) add_subdirectory(src/sign_detector) +add_subdirectory(src/stopbar_detector) add_subdirectory(src/urc_image_merger) add_subdirectory(src/urc_turn_action) diff --git a/rr_iarrc/launch/perception/stopbar_detector.launch b/rr_iarrc/launch/perception/stopbar_detector.launch new file mode 100644 index 000000000..bd83a4a36 --- /dev/null +++ b/rr_iarrc/launch/perception/stopbar_detector.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + 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..43e8067a0 --- /dev/null +++ b/rr_iarrc/src/stopbar_detector/stopbar_detector.cpp @@ -0,0 +1,172 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +cv_bridge::CvImagePtr cv_ptrLine; +ros::Publisher pub_line; +ros::Publisher pub_near_stopbar; +ros::Publisher pub_angle; + +std_msgs::Bool stop_bar_near; +std_msgs::Float64 stop_bar_angle; + +double stopBarGoalAngle; +double stopBarGoalAngleRange; +double stopBarTriggerDistance; +int houghThreshold; +double houghMinLineLength; +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) { + return cv::getStructuringElement(cv::MORPH_RECT, cv::Size(x, y)); +} + +/* + * 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 debug The debug image + */ +bool findStopBarFromHough(cv::Mat& frame, cv::Mat& debug, double& stopBarAngle) { + int ddepth = CV_8UC1; + 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 + 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(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 distanceY = p2.y - p1.y; + double currAngle = fabs(atan(distanceY / distanceX)); // in radians + cv::Point midpoint = (p1 + p2) * 0.5; + 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(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 + 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 + 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, 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, frame.rows - dist / 2), + cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 255, 0), 1); + + stopBarAngle = currAngle; + if (speed != 0) { + double timeToStopBar = (dist / speed) * sleepConstant; + ros::Duration(timeToStopBar).sleep(); + } + return true; // stop bar detected close to us! + } + } + } + return false; // 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; + bool stopBarDetected = findStopBarFromHough(frame, debug, stopBarAngle); + + // 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; + pub_angle.publish(stop_bar_angle); + } + pub_near_stopbar.publish(stop_bar_near); + if (pub_line.getNumSubscribers() > 0) { + sensor_msgs::Image outmsg; + cv_ptrLine->image = debug; + cv_ptrLine->encoding = "bgr8"; + cv_ptrLine->toImageMsg(outmsg); + pub_line.publish(outmsg); + } +} + +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; + + 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")); + + 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 + 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); + nhp.param("houghMinLineLength", houghMinLineLength, 0.0); + nhp.param("houghMaxLineGap", houghMaxLineGap, 0.0); + + pub_line = nh.advertise("/stopbar_detector/stop_bar", + 1); // debug publish of image + 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(); + return 0; +} \ No newline at end of file