From: Erik Andresen Date: Fri, 15 May 2015 12:00:56 +0000 (+0200) Subject: initial hpec sav based path following X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=eef02300bd4b99a11e6a1756f7b8d4e25c6c0908;p=ros_wild_thumper.git initial hpec sav based path following --- diff --git a/CMakeLists.txt b/CMakeLists.txt index c35ceaf..5369f92 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(wild_thumper) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED) +find_package(catkin REQUIRED sensor_msgs cv_bridge roscpp std_msgs image_transport dynamic_reconfigure) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -66,6 +66,10 @@ find_package(catkin REQUIRED) # std_msgs # Or other packages containing msgs # ) +generate_dynamic_reconfigure_options( + cfg/path_following.cfg +) + ################################### ## catkin specific configuration ## ################################### @@ -78,7 +82,7 @@ find_package(catkin REQUIRED) catkin_package( # INCLUDE_DIRS include # LIBRARIES wild_thumper -# CATKIN_DEPENDS other_catkin_pkg +# CATKIN_DEPENDS sensor_msgs # DEPENDS system_lib ) @@ -96,16 +100,16 @@ catkin_package( # ) ## Declare a cpp executable -# add_executable(wild_thumper_node src/wild_thumper_node.cpp) +add_executable(path_following src/path_following.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes -# add_dependencies(wild_thumper_node wild_thumper_generate_messages_cpp) +add_dependencies(path_following ${PROJECT_NAME}_gencfg) ## Specify libraries to link a library or executable target against -# target_link_libraries(wild_thumper_node -# ${catkin_LIBRARIES} -# ) +target_link_libraries(path_following + ${catkin_LIBRARIES} +) ############# ## Install ## diff --git a/cfg/path_following.cfg b/cfg/path_following.cfg new file mode 100755 index 0000000..901b0bf --- /dev/null +++ b/cfg/path_following.cfg @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("binary_threshold", int_t, 0, "Binary Threshold", 192, 0, 255) +gen.add("ht_min_points", int_t, 0, "HT Min Points", 30, 0, 1000) +gen.add("roi_y", int_t, 0, "ROI Y (cm)", 70, 0, 1000) +gen.add("road_distance_cm", double_t, 0, "Road distance (cm)", 15, 0, 1000) +gen.add("lad_cm", double_t, 0, "Look ahead distance (cm)", 60, 0, 1000) +gen.add("speed_m_s", double_t, 0, "Speed m/s", 0, 0, 1) + +exit(gen.generate("wild_thumper", "path_following", "PathFollowing")) diff --git a/cfg/transformation_matrix.yml b/cfg/transformation_matrix.yml new file mode 100644 index 0000000..17e9ee5 --- /dev/null +++ b/cfg/transformation_matrix.yml @@ -0,0 +1,6 @@ +%YAML:1.0 +transformation_matrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 1.27755347e-02, 7.48846231e-02, -2.97173802e-01, -2.46883581e-03, 1.59522560e-01, -5.47704021e-01, -5.19536325e-06, 2.98962964e-04, 1.52353391e-02] diff --git a/data/chessboard.jpg b/data/chessboard.jpg new file mode 100644 index 0000000..676cf20 Binary files /dev/null and b/data/chessboard.jpg differ diff --git a/package.xml b/package.xml index cc6778c..395909f 100644 --- a/package.xml +++ b/package.xml @@ -40,7 +40,11 @@ catkin - + sensor_msgs + cv_bridge + roscpp + std_msgs + image_transport @@ -50,4 +54,4 @@ - \ No newline at end of file + diff --git a/scripts/imagefeed.py b/scripts/imagefeed.py new file mode 100755 index 0000000..31a0149 --- /dev/null +++ b/scripts/imagefeed.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python +# Lucas Walter +# GNU GPLv3 +# Load an image from disk and publish it repeatedly + +import cv2 +import numpy as np +import rospy +import sys +from sensor_msgs.msg import Image + +if __name__ == '__main__': + rospy.init_node('image_publish') + + name = sys.argv[1] + image = cv2.imread(name) + #cv2.imshow("im", image) + #cv2.waitKey(5) + + hz = rospy.get_param("~rate", 1) + rate = rospy.Rate(hz) + + pub = rospy.Publisher('/test/image', Image, queue_size=1) + + msg = Image() + msg.header.stamp = rospy.Time.now() + msg.encoding = 'bgr8' + msg.height = image.shape[0] + msg.width = image.shape[1] + msg.step = image.shape[1] * 3 + msg.data = image.tostring() + pub.publish(msg) + + while not rospy.is_shutdown(): + pub.publish(msg) + rate.sleep() diff --git a/src/path_following.cpp b/src/path_following.cpp new file mode 100644 index 0000000..7bbd7ef --- /dev/null +++ b/src/path_following.cpp @@ -0,0 +1,234 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace cv; + +static const std::string OPENCV_WINDOW = "Image window"; +static const Scalar& red = CV_RGB(255,0,0); +static const Scalar& orange = CV_RGB(255,165,0); +static const Scalar& green = CV_RGB(0,255,0); +static const Scalar& blue = CV_RGB(0,0,255); +static const Scalar& purple = CV_RGB(128,0,128); + +class PathFollower +{ + ros::NodeHandle nh; + image_transport::ImageTransport it; + image_transport::Subscriber image_sub; + ros::Publisher cmdvel_pub; + int roi_x; + int roi_y; + int binary_threshold; + int ht_min_points; + int roi_width, roi_height; + double x_px_per_cm, y_px_per_cm, y_offset_cm; + int x_offset_px; + float lap_x_cm, lap_y_cm; + float road_distance_cm, lad_cm; + int img_vsize, img_hsize; + double axial_dist_cm; + float speed_m_s; + Mat transformation_matrix; + dynamic_reconfigure::Server server; + + public: + PathFollower() : it(nh) + { + ros::NodeHandle pnh("~"); + FileStorage fs(ros::package::getPath("wild_thumper") + "/cfg/transformation_matrix.yml", FileStorage::READ); + dynamic_reconfigure::Server::CallbackType f; + + fs["transformation_matrix"] >> transformation_matrix; + pnh.param("roi_x_init", roi_x, 280); + pnh.param("roi_width", roi_width, 50); + pnh.param("roi_height", roi_height, 50); + pnh.param("x_px_per_cm", x_px_per_cm, 2.778); + pnh.param("y_px_per_cm", y_px_per_cm, 2.778); + pnh.param("y_offset_cm", y_offset_cm, 23.5); + pnh.param("img_vsize", img_vsize, 480); + pnh.param("img_hsize", img_hsize, 640); + pnh.param("x_offset_px", x_offset_px, 65); + pnh.param("axial_dist_cm", axial_dist_cm, 25.4); + lap_x_cm=0; + lap_y_cm=0; + f = boost::bind(&PathFollower::dynreconf_callback, this, _1, _2); + server.setCallback(f); + + // Subscrive to input video feed and publish output video feed + image_sub = it.subscribe("/camera/rgb/image_raw", 1, &PathFollower::imageCb, this); + cmdvel_pub = nh.advertise("/cmd_vel", 10); + + fs.release(); + + cv::namedWindow(OPENCV_WINDOW); + } + + ~PathFollower() + { + cv::destroyWindow(OPENCV_WINDOW); + } + + int pathfinder(cv::Mat &frame, float &r, float &phi) + { + cv::vector lines; + cv::warpPerspective(frame, frame, transformation_matrix, frame.size(), INTER_LINEAR); + Mat roi(frame, Rect(roi_x, roi_y, roi_width, roi_height)); + + cv::threshold(roi, roi, binary_threshold, 255, CV_THRESH_BINARY); + cv::erode(roi, roi, cv::Mat(), cv::Point(-1, -1), 1); + cv::HoughLines(roi, lines, 1, CV_PI/90, ht_min_points, 0, 0); + + if (lines.size() > 0) { // use first result + r = lines[0][0]; + phi = lines[0][1]; + + if (phi > CV_PI/2) { // keep in +-90° + phi = phi-CV_PI; + r = -r; + } + + // Update dynamic ROI + roi_x = roi_x - roi_width/2 + r; + } + + return lines.size(); + } + + void calc_lap(float r, float phi) { + float roi_x_h, lap_x_h, lap_y_h; + + // 1. rotate ROI by phi (in Hilfskoordinatensystem) + roi_x_h = (cos(-phi)*px_to_cm_x(roi_x) + -sin(-phi)*px_to_cm_y(roi_y)); + + // 2. lap_h + lap_x_h = roi_x_h + r - road_distance_cm; + // lap_x_h + lap_y_h = sqrt(lad_cm*lad_cm - lap_x_h*lap_x_h); + + // 3. rotate LAP back by phi + lap_x_cm = (cos(phi)*lap_x_h + -sin(phi)*lap_y_h); + lap_y_cm = (sin(phi)*lap_x_h + cos(phi)*lap_y_h); + } + + float pure_pursuit(float r, float phi) { + calc_lap(r, phi); + + return -atan((2*lap_x_cm*axial_dist_cm)/((float)lad_cm*lad_cm)); + } + + void imageCb(const sensor_msgs::ImageConstPtr& msg) + { + cv_bridge::CvImagePtr cv_ptr; + cv::Mat cimg; + float r, phi; + int htvalid; + float alpha; + + try + { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + htvalid = pathfinder(cv_ptr->image, r, phi); + + cv::cvtColor(cv_ptr->image, cimg, CV_GRAY2RGB); + cv::rectangle(cimg, cv::Point(roi_x, roi_y), cv::Point(roi_x+roi_width, roi_y+roi_height), red, 1); + + // grid + cv::line(cimg, Point(0, cm_to_px_y(50)), Point(img_hsize, cm_to_px_y(50)), blue, 1); + putText(cimg, " 50cm", Point(img_hsize-60, cm_to_px_y(50)+15), FONT_HERSHEY_PLAIN, 1.0, blue); + cv::line(cimg, Point(0, cm_to_px_y(60)), Point(img_hsize, cm_to_px_y(60)), blue, 1); + putText(cimg, " 60cm", Point(img_hsize-60, cm_to_px_y(60)+15), FONT_HERSHEY_PLAIN, 1.0, blue); + cv::line(cimg, Point(0, cm_to_px_y(70)), Point(img_hsize, cm_to_px_y(70)), blue, 1); + putText(cimg, " 70cm", Point(img_hsize-60, cm_to_px_y(70)+15), FONT_HERSHEY_PLAIN, 1.0, blue); + cv::line(cimg, Point(0, cm_to_px_y(80)), Point(img_hsize, cm_to_px_y(80)), blue, 1); + putText(cimg, " 80cm", Point(img_hsize-60, cm_to_px_y(80)+15), FONT_HERSHEY_PLAIN, 1.0, blue); + cv::line(cimg, Point(0, cm_to_px_y(100)), Point(img_hsize, cm_to_px_y(100)), blue, 1); + putText(cimg, "100cm", Point(img_hsize-60, cm_to_px_y(100)+15), FONT_HERSHEY_PLAIN, 1.0, blue); + // distance + cv::line(cimg, Point(cm_to_px_x(road_distance_cm), 0), Point(cm_to_px_x(road_distance_cm), img_vsize), purple, 1); + + if (htvalid) { + double a = cos(phi), b = sin(phi); + double x0 = a*r + roi_x; + double y0 = b*r + roi_y; + Point pt1, pt2; + pt1.y = 0; + pt1.x = r/(cos(phi)); + pt2.y = roi_height; + pt2.x = (r - pt2.y*sin(phi)) / (cos(phi)); + cv::line(cimg, Point(roi_x, roi_y) + pt1, Point(roi_x, roi_y) + pt2, orange, 2); + + alpha = pure_pursuit(px_to_cm_x(roi_x + r), phi); + ROS_INFO("alpha=%.2f deg", alpha*180/M_PI); + if (speed_m_s > 0) { + set_vel(speed_m_s, alpha); + } + } + + cv::circle(cimg, Point(cm_to_px_x(lap_x_cm), cm_to_px_y(lap_y_cm)), 5, green, -1); + + // Update GUI Window + cv::imshow(OPENCV_WINDOW, cimg); + cv::waitKey(3); + } + + int cm_to_px_x(float cm) { + return round(cm * x_px_per_cm + img_hsize/2 - x_offset_px); + } + + double px_to_cm_x(int px) { + return (px-img_hsize/2+x_offset_px) / x_px_per_cm; + } + + int cm_to_px_y(float cm) { + return round((-cm+y_offset_cm) * y_px_per_cm + img_vsize); + } + + double px_to_cm_y(int px) { + return (img_vsize-px) / y_px_per_cm + y_offset_cm; + } + + void dynreconf_callback(wild_thumper::PathFollowingConfig &config, uint32_t level) { + int px; + + binary_threshold = config.binary_threshold; + ht_min_points = config.ht_min_points; + px = cm_to_px_y(config.roi_y); + if (px > 0 && px < img_vsize - roi_height) roi_y = px; + else ROS_ERROR("Bad roi_y position %.2fcm", roi_y); + road_distance_cm = config.road_distance_cm; + lad_cm = config.lad_cm; + speed_m_s = config.speed_m_s; + } + + void set_vel(float speed, float alpha) { + geometry_msgs::Twist msg; + + msg.linear.x = speed; + msg.angular.z = alpha; + + cmdvel_pub.publish(msg); + } +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "path_following"); + + PathFollower ic; + ros::spin(); + return 0; +}