src/path_following.cpp

   1 #include <ros/ros.h>
   2 #include <image_transport/image_transport.h>
   3 #include <cv_bridge/cv_bridge.h>
   4 #include <sensor_msgs/image_encodings.h>
   5 #include <opencv2/imgproc/imgproc.hpp>
   6 #include <opencv2/highgui/highgui.hpp>
   7 #include <ros/package.h>
   8 #include <dynamic_reconfigure/server.h>
   9 #include <wild_thumper/PathFollowingConfig.h>
  10 #include <geometry_msgs/Twist.h>
  11
  12 using namespace cv;
  13
  14 static const std::string OPENCV_WINDOW = "Image window";
  15 static const Scalar& red = CV_RGB(255,0,0);
  16 static const Scalar& orange = CV_RGB(255,165,0);
  17 static const Scalar& green = CV_RGB(0,255,0);
  18 static const Scalar& blue = CV_RGB(0,0,255);
  19 static const Scalar& purple = CV_RGB(128,0,128);
  20
  21 class PathFollower
  22 {
  23         ros::NodeHandle nh;
  24         image_transport::ImageTransport it;
  25         image_transport::Subscriber image_sub;
  26         ros::Publisher cmdvel_pub;
  27         int roi_x;
  28         int roi_y;
  29         int binary_threshold;
  30         int ht_min_points;
  31         int roi_width, roi_height;
  32         double x_px_per_cm, y_px_per_cm, y_offset_cm;
  33         int x_offset_px;
  34         float lap_x_cm, lap_y_cm;
  35         float road_distance_cm, lad_cm;
  36         int img_vsize, img_hsize;
  37         double axial_dist_cm;
  38         float speed_m_s;
  39         Mat transformation_matrix;
  40         dynamic_reconfigure::Server<wild_thumper::PathFollowingConfig> server;
  41
  42         public:
  43         PathFollower() : it(nh)
  44         {
  45                 ros::NodeHandle pnh("~");
  46                 FileStorage fs(ros::package::getPath("wild_thumper") + "/config/transformation_matrix.yml", FileStorage::READ);
  47                 dynamic_reconfigure::Server<wild_thumper::PathFollowingConfig>::CallbackType f;
  48
  49                 fs["transformation_matrix"] >> transformation_matrix;
  50                 pnh.param("roi_x_init", roi_x, 280);
  51                 pnh.param("roi_width", roi_width, 50);
  52                 pnh.param("roi_height", roi_height, 50);
  53                 pnh.param("x_px_per_cm", x_px_per_cm, 2.778);
  54                 pnh.param("y_px_per_cm", y_px_per_cm, 2.778);
  55                 pnh.param("y_offset_cm", y_offset_cm, 23.5);
  56                 pnh.param("img_vsize", img_vsize, 480);
  57                 pnh.param("img_hsize", img_hsize, 640);
  58                 pnh.param("x_offset_px", x_offset_px, 65);
  59                 pnh.param("axial_dist_cm", axial_dist_cm, 25.4);
  60                 lap_x_cm=0;
  61                 lap_y_cm=0;
  62                 f = boost::bind(&PathFollower::dynreconf_callback, this, _1, _2);
  63                 server.setCallback(f);
  64
  65                 // Subscrive to input video feed and publish output video feed
  66                 image_sub = it.subscribe("/camera/rgb/image_raw", 1, &PathFollower::imageCb, this);
  67                 cmdvel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
  68
  69                 fs.release();
  70
  71                 cv::namedWindow(OPENCV_WINDOW);
  72         }
  73
  74         ~PathFollower()
  75         {
  76                 cv::destroyWindow(OPENCV_WINDOW);
  77         }
  78
  79         int pathfinder(cv::Mat &frame, float &r, float &phi)
  80         {
  81                 std::vector<cv::Vec2f> lines;
  82                 cv::warpPerspective(frame, frame, transformation_matrix, frame.size(), INTER_LINEAR);
  83                 Mat roi(frame, Rect(roi_x, roi_y, roi_width, roi_height));
  84
  85                 cv::threshold(roi, roi, binary_threshold, 255, CV_THRESH_BINARY);
  86                 cv::erode(roi, roi, cv::Mat(), cv::Point(-1, -1), 1);
  87                 cv::HoughLines(roi, lines, 1, CV_PI/90, ht_min_points, 0, 0);
  88
  89                 if (lines.size() > 0) { // use first result
  90                         r = lines[0][0];
  91                         phi = lines[0][1];
  92
  93                         if (phi > CV_PI/2) { // keep in +-90°
  94                                 phi = phi-CV_PI;
  95                                 r = -r;
  96                         }
  97
  98                         // Update dynamic ROI
  99                         roi_x = roi_x - roi_width/2 + r;
 100                 }
 101
 102                 return lines.size();
 103         }
 104
 105         void calc_lap(float r, float phi) {
 106                 float roi_x_h, lap_x_h, lap_y_h;
 107
 108                 // 1. rotate ROI by phi (in Hilfskoordinatensystem)
 109                 roi_x_h = (cos(-phi)*px_to_cm_x(roi_x) + -sin(-phi)*px_to_cm_y(roi_y));
 110
 111                 // 2. lap_h
 112                 lap_x_h = roi_x_h + r - road_distance_cm;
 113                 // lap_x_h
 114                 lap_y_h = sqrt(lad_cm*lad_cm - lap_x_h*lap_x_h);
 115
 116                 // 3. rotate LAP back by phi
 117                 lap_x_cm = (cos(phi)*lap_x_h + -sin(phi)*lap_y_h);
 118                 lap_y_cm = (sin(phi)*lap_x_h + cos(phi)*lap_y_h);
 119         }
 120
 121         float pure_pursuit(float r, float phi) {
 122                 calc_lap(r, phi);
 123
 124                 return -atan((2*lap_x_cm*axial_dist_cm)/((float)lad_cm*lad_cm));
 125         }
 126
 127         void imageCb(const sensor_msgs::ImageConstPtr& msg)
 128         {
 129                 cv_bridge::CvImagePtr cv_ptr;
 130                 cv::Mat cimg;
 131                 float r, phi;
 132                 int htvalid;
 133                 float alpha;
 134
 135                 try
 136                 {
 137                         cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8);
 138                 }
 139                 catch (cv_bridge::Exception& e)
 140                 {
 141                         ROS_ERROR("cv_bridge exception: %s", e.what());
 142                         return;
 143                 }
 144                 htvalid = pathfinder(cv_ptr->image, r, phi);
 145
 146                 cv::cvtColor(cv_ptr->image, cimg, CV_GRAY2RGB);
 147                 cv::rectangle(cimg, cv::Point(roi_x, roi_y), cv::Point(roi_x+roi_width, roi_y+roi_height), red, 1);
 148
 149                 // grid
 150                 cv::line(cimg, Point(0, cm_to_px_y(50)), Point(img_hsize, cm_to_px_y(50)), blue, 1);
 151                 putText(cimg, " 50cm", Point(img_hsize-60, cm_to_px_y(50)+15), FONT_HERSHEY_PLAIN, 1.0, blue);
 152                 cv::line(cimg, Point(0, cm_to_px_y(60)), Point(img_hsize, cm_to_px_y(60)), blue, 1);
 153                 putText(cimg, " 60cm", Point(img_hsize-60, cm_to_px_y(60)+15), FONT_HERSHEY_PLAIN, 1.0, blue);
 154                 cv::line(cimg, Point(0, cm_to_px_y(70)), Point(img_hsize, cm_to_px_y(70)), blue, 1);
 155                 putText(cimg, " 70cm", Point(img_hsize-60, cm_to_px_y(70)+15), FONT_HERSHEY_PLAIN, 1.0, blue);
 156                 cv::line(cimg, Point(0, cm_to_px_y(80)), Point(img_hsize, cm_to_px_y(80)), blue, 1);
 157                 putText(cimg, " 80cm", Point(img_hsize-60, cm_to_px_y(80)+15), FONT_HERSHEY_PLAIN, 1.0, blue);
 158                 cv::line(cimg, Point(0, cm_to_px_y(100)), Point(img_hsize, cm_to_px_y(100)), blue, 1);
 159                 putText(cimg, "100cm", Point(img_hsize-60, cm_to_px_y(100)+15), FONT_HERSHEY_PLAIN, 1.0, blue);
 160                 // distance
 161                 cv::line(cimg, Point(cm_to_px_x(road_distance_cm), 0), Point(cm_to_px_x(road_distance_cm), img_vsize), purple, 1);
 162
 163                 if (htvalid) {
 164                         double a = cos(phi), b = sin(phi);
 165                         double x0 = a*r + roi_x;
 166                         double y0 = b*r + roi_y;
 167                         Point pt1, pt2;
 168                         pt1.y = 0;
 169                         pt1.x = r/(cos(phi));
 170                         pt2.y = roi_height;
 171                         pt2.x = (r - pt2.y*sin(phi)) / (cos(phi));
 172                         cv::line(cimg, Point(roi_x, roi_y) + pt1, Point(roi_x, roi_y) + pt2, orange, 2);
 173
 174                         alpha = pure_pursuit(px_to_cm_x(roi_x + r), phi);
 175                         ROS_INFO("alpha=%.2f deg", alpha*180/M_PI);
 176                         if (speed_m_s > 0) {
 177                                 set_vel(speed_m_s, alpha);
 178                         }
 179                 }
 180
 181                 cv::circle(cimg, Point(cm_to_px_x(lap_x_cm), cm_to_px_y(lap_y_cm)), 5, green, -1);
 182
 183                 // Update GUI Window
 184                 cv::imshow(OPENCV_WINDOW, cimg);
 185                 cv::waitKey(3);
 186         }
 187
 188         int cm_to_px_x(float cm) {
 189                 return round(cm * x_px_per_cm + img_hsize/2 - x_offset_px);
 190         }
 191
 192         double px_to_cm_x(int px) {
 193                 return (px-img_hsize/2+x_offset_px) / x_px_per_cm;
 194         }
 195
 196         int cm_to_px_y(float cm) {
 197                 return round((-cm+y_offset_cm) * y_px_per_cm + img_vsize);
 198         }
 199
 200         double px_to_cm_y(int px) {
 201                 return (img_vsize-px) / y_px_per_cm + y_offset_cm;
 202         }
 203
 204         void dynreconf_callback(wild_thumper::PathFollowingConfig &config, uint32_t level) {
 205                 int px;
 206
 207                 binary_threshold = config.binary_threshold;
 208                 ht_min_points = config.ht_min_points;
 209                 px = cm_to_px_y(config.roi_y);
 210                 if (px > 0 && px < img_vsize - roi_height) roi_y = px;
 211                 else ROS_ERROR("Bad roi_y position %.2fcm", roi_y);
 212                 road_distance_cm = config.road_distance_cm;
 213                 lad_cm = config.lad_cm;
 214                 speed_m_s = config.speed_m_s;
 215         }
 216
 217         void set_vel(float speed, float alpha) {
 218                 geometry_msgs::Twist msg;
 219
 220                 msg.linear.x = speed;
 221                 msg.angular.z = alpha;
 222
 223                 cmdvel_pub.publish(msg);
 224         }
 225 };
 226
 227 int main(int argc, char** argv)
 228 {
 229         ros::init(argc, argv, "path_following");
 230
 231         PathFollower ic;
 232         ros::spin();
 233         return 0;
 234 }