├── .readme ├── figure3.png ├── husky_test.gif ├── husky_test_nav.gif ├── motivation.png ├── motivation_old.png ├── vs_em.png ├── vs_graph.png └── vs_poster.png ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include ├── agribot_types.h ├── agribot_vs.h └── agribot_vs_nodehandler.h ├── launch ├── camera_umd.launch └── visualservoing.launch ├── msg └── vs_msg.msg ├── package.xml ├── params └── agribot_vs_run.yaml └── src ├── agribot_vs.cpp ├── agribot_vs_node.cpp └── agribot_vs_nodehandler.cpp /.readme/figure3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/visual-crop-row-navigation/6ddfa6cc183c4d21eda64abe2003291ecffeb4f3/.readme/figure3.png -------------------------------------------------------------------------------- /.readme/husky_test.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/visual-crop-row-navigation/6ddfa6cc183c4d21eda64abe2003291ecffeb4f3/.readme/husky_test.gif -------------------------------------------------------------------------------- /.readme/husky_test_nav.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/visual-crop-row-navigation/6ddfa6cc183c4d21eda64abe2003291ecffeb4f3/.readme/husky_test_nav.gif -------------------------------------------------------------------------------- /.readme/motivation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/visual-crop-row-navigation/6ddfa6cc183c4d21eda64abe2003291ecffeb4f3/.readme/motivation.png -------------------------------------------------------------------------------- /.readme/motivation_old.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/visual-crop-row-navigation/6ddfa6cc183c4d21eda64abe2003291ecffeb4f3/.readme/motivation_old.png -------------------------------------------------------------------------------- /.readme/vs_em.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/visual-crop-row-navigation/6ddfa6cc183c4d21eda64abe2003291ecffeb4f3/.readme/vs_em.png -------------------------------------------------------------------------------- /.readme/vs_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/visual-crop-row-navigation/6ddfa6cc183c4d21eda64abe2003291ecffeb4f3/.readme/vs_graph.png -------------------------------------------------------------------------------- /.readme/vs_poster.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/visual-crop-row-navigation/6ddfa6cc183c4d21eda64abe2003291ecffeb4f3/.readme/vs_poster.png -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(visual_crop_row_navigation) 3 | 4 | # c++11 5 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | genmsg 10 | cmake_modules 11 | message_generation 12 | dynamic_reconfigure 13 | std_msgs 14 | cv_bridge 15 | sensor_msgs 16 | image_transport 17 | ) 18 | 19 | find_package( PkgConfig ) 20 | find_package( OpenCV REQUIRED ) 21 | 22 | add_message_files(FILES vs_msg.msg) 23 | 24 | generate_messages(DEPENDENCIES std_msgs) 25 | 26 | set (CMAKE_CXX_STANDARD 11) 27 | if(NOT CMAKE_BUILD_TYPE) 28 | set(CMAKE_BUILD_TYPE Release) 29 | endif() 30 | set(CMAKE_CXX_FLAGS "-Wall -Wextra -fPIC") 31 | set(CMAKE_CXX_FLAGS_DEBUG "-g") 32 | set(CMAKE_CXX_FLAGS_RELEASE "-O3") 33 | 34 | find_package (Eigen3 3.3 REQUIRED NO_MODULE) 35 | pkg_check_modules( EIGEN3 REQUIRED eigen3 ) 36 | 37 | include_directories( 38 | include 39 | ${catkin_INCLUDE_DIRS} 40 | ${EIGEN3_INCLUDE_DIRS} 41 | ) 42 | 43 | catkin_package( 44 | INCLUDE_DIRS 45 | include 46 | LIBRARIES 47 | ${PROJECT_NAME}_core 48 | CATKIN_DEPENDS 49 | sensor_msgs 50 | std_msgs 51 | message_runtime 52 | ) 53 | 54 | ## Declare a cpp library 55 | add_library(${PROJECT_NAME}_core 56 | src/agribot_vs.cpp 57 | ) 58 | 59 | ## Declare cpp executables 60 | add_executable(agribot_vs_node 61 | src/agribot_vs_node.cpp 62 | src/agribot_vs_nodehandler.cpp 63 | ) 64 | 65 | target_link_libraries(agribot_vs_node 66 | ${PROJECT_NAME}_core 67 | ${catkin_LIBRARIES} 68 | ${OpenCV_LIBS} 69 | Eigen3::Eigen 70 | ) 71 | 72 | add_dependencies(agribot_vs_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) 73 | add_dependencies(${PROJECT_NAME}_core ${${PROJECT_NAME}_EXPORTED_TARGETS}) 74 | 75 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Photogrammetry & Robotics Bonn 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Visual Crop Row Navigation 2 | 3 |
4 | visual_servoing_husky 5 |
6 | 7 | 8 | **Update** 9 | 10 | A python based implementation for Multi-crop-row navigation can be found here [visual-multi-crop-row-navigation](https://github.com/Agricultural-Robotics-Bonn/visual-multi-crop-row-navigation) 11 | 12 |
13 | 14 | [![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/z2Cb2FFZ2aU/0.jpg)](https://www.youtube.com/watch?v=z2Cb2FFZ2aU) 15 | 16 |
17 | 18 | This is a visual-servoing based robot navigation framework tailored for navigating in row-crop fields. 19 | It uses the images from two on-board cameras and exploits the regular crop-row structure present in the fields for navigation, without performing explicit localization or mapping. It allows the robot to follow the crop-rows accurately and handles the switch to the next row seamlessly within the same framework. 20 | 21 | This implementation uses C++ and ROS and has been tested in different environments both in simulation and in real world and on diverse robotic platforms. 22 | 23 | This work has been developed @ [IPB](http://www.ipb.uni-bonn.de/), University of Bonn. 24 | 25 | Check out the [video1](https://www.youtube.com/watch?v=uO6cgBqKBas), [video2](https://youtu.be/KkCVQAhzS4g) of our robot following this approach to navigate on a test row-crop field. 26 | 27 |
28 | husky_navigationhusky_navigation 30 | 33 |
34 | 35 | 36 | ## Features 37 | 38 | - No maps or localization required. 39 | - Running on embedded controllers with limit processing power (Odroid, Raspberry Pi). 40 | - Simulation environment in Gazebo. 41 | - Robot and cameras agnostic. 42 | 43 | ## Robotic setup 44 | 45 | This navigation framework is designed for mobile robots equipped with two cameras mounted respectively looking to the front and to the back of the robot as illustrated in the picture below. 46 | 47 |
48 | agribot_3d 49 | camera_img 50 |
51 | 52 | A complete Gazebo simulation package is provided in [agribot_robot](https://github.com/PRBonn/agribot) repository including simulated row-crop fields and robot for testing the navigation framework. 53 | 54 |
55 | husky_navigation 56 | gazebo_navigation 57 |
58 | 59 | ## Dependencies 60 | 61 | - c++11 62 | - catkin 63 | - opencv >= 2.4 64 | - Eigen >= 3.3 65 | 66 | ## How to build and run 67 | 68 | 1. Clone the package into your *catkin_ws* 69 | ```bash 70 | cd ~/catkin_ws/src 71 | git clone https://github.com/PRBonn/visual_crop_row_navigation.git 72 | ``` 73 | 2. Build the package 74 | ```bash 75 | cd ~/catkin_ws 76 | catkin build visual_crop_row_navigation 77 | ``` 78 | 3. Run ROS driver to stream images from the robot's cameras, for example using [usb_cam](http://wiki.ros.org/usb_cam) 79 | 83 | 4. Run visual servoing navigation 84 | ```bash 85 | roslaunch visual_crop_row_navigation visualservoing.launch 86 | ``` 87 | 88 | Successfully tested using: 89 | - Ubuntu 16.04 90 | - ROS kinetic 91 | 92 | ## Test data 93 | 94 | Download the bagfile used for our experiments [here](). 95 | 96 | ## Simulation 97 | 98 | Simultion and robot packages can be found in [Agribot repo](https://github.com/PRBonn/agribot) 99 | 100 | --- 101 | 102 | ## Citation 103 | if you use this project in your recent works please refernce to it by: 104 | 105 | ```bash 106 | 107 | @article{ahmadi2021towards, 108 | title={Towards Autonomous Crop-Agnostic Visual Navigation in Arable Fields}, 109 | author={Ahmadi, Alireza and Halstead, Michael and McCool, Chris}, 110 | journal={arXiv preprint arXiv:2109.11936}, 111 | year={2021} 112 | } 113 | 114 | @inproceedings{ahmadi2020visual, 115 | title={Visual servoing-based navigation for monitoring row-crop fields}, 116 | author={Ahmadi, Alireza and Nardi, Lorenzo and Chebrolu, Nived and Stachniss, Cyrill}, 117 | booktitle={2020 IEEE International Conference on Robotics and Automation (ICRA)}, 118 | pages={4920--4926}, 119 | year={2020}, 120 | organization={IEEE} 121 | } 122 | 123 | ``` 124 | 125 | ## Acknowledgments 126 | This work has been supported by the German Research Foundation under Germany’s Excellence Strategy, EXC-2070 - 390732324 ([PhenoRob](http://www.phenorob.de/)) and [Bonn AgRobotics Group](http://agrobotics.uni-bonn.de/) 127 | -------------------------------------------------------------------------------- /include/agribot_types.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************************/ 2 | /* Paper: Visual-Servoing based Navigation for Monitoring Row-Crop Fields */ 3 | /* Alireza Ahmadi, Lorenzo Nardi, Nived Chebrolu, Chis McCool, Cyrill Stachniss */ 4 | /* All authors are with the University of Bonn, Germany */ 5 | /* maintainer: Alireza Ahmadi */ 6 | /* (Alireza.Ahmadi@uni-bonn.de / http://alirezaahmadi.xyz) */ 7 | /***************************************************************************************/ 8 | 9 | #pragma once 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace cv; 15 | using namespace std; 16 | using namespace Eigen; 17 | 18 | namespace agribot_vs { 19 | 20 | /** 21 | * @brief Neighbourhood type containing 22 | * features of each Neighbourhood 23 | * 24 | */ 25 | struct Neighbourhood { 26 | int Xc; 27 | int Yc; 28 | int L; // Length (and Width) of the neighbourhood box 29 | int H; 30 | int offset; // Offset in pixels to capture next row. (TO DO: Compute using rho and crop-row distance) 31 | float shift; // Offset in pixels including the direction 32 | } ; 33 | 34 | 35 | /** 36 | * @brief object of camera containing features and 37 | * neighbourhood properties 38 | */ 39 | struct camera{ 40 | Mat image; 41 | vector id; 42 | vector nh_id; 43 | vector points; 44 | vector nh_points; 45 | vector> contours; 46 | vector lines; 47 | // set camera Intrinsics 48 | int height; 49 | int width; 50 | int f_mm; // focal length (in mm) 51 | int s_w; // sensor width (in mm) 52 | vector nh_ex; 53 | Neighbourhood nh; 54 | } ; 55 | 56 | } -------------------------------------------------------------------------------- /include/agribot_vs.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************************/ 2 | /* Paper: Visual-Servoing based Navigation for Monitoring Row-Crop Fields */ 3 | /* Alireza Ahmadi, Lorenzo Nardi, Nived Chebrolu, Chis McCool, Cyrill Stachniss */ 4 | /* All authors are with the University of Bonn, Germany */ 5 | /* maintainer: Alireza Ahmadi */ 6 | /* (Alireza.Ahmadi@uni-bonn.de / http://alirezaahmadi.xyz) */ 7 | /***************************************************************************************/ 8 | 9 | #pragma once 10 | 11 | #include "ros/ros.h" 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "cv_bridge/cv_bridge.h" 23 | #include "opencv2/highgui/highgui.hpp" 24 | #include 25 | #include 26 | #include 27 | 28 | #include "sensor_msgs/Image.h" 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include "nav_msgs/Odometry.h" 35 | #include "sensor_msgs/Imu.h" 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include "visual_crop_row_navigation/vs_msg.h" 44 | #include "agribot_types.h" 45 | 46 | #define DEG2RAD 0.0174533 47 | #define RAD2DEG 57.2958 48 | 49 | #define sind(x) (sin(fmod((x),360) * M_PI / 180)) 50 | #define cosd(x) (cos(fmod((x),360) * M_PI / 180)) 51 | 52 | using namespace cv; 53 | using namespace std; 54 | using namespace Eigen; 55 | 56 | namespace agribot_vs { 57 | /** 58 | * @brief main class which offers functions to perform visualServoing 59 | * 60 | */ 61 | class AgribotVS { 62 | public: 63 | /** 64 | * @brief Construct a new Agribot V S:: Agribot V S object 65 | * 66 | */ 67 | AgribotVS(); 68 | /** 69 | * @brief Destroy the Agribot V S:: Agribot V S object 70 | * 71 | */ 72 | ~AgribotVS(); 73 | 74 | /** 75 | * @brief - function to load run params 76 | * 77 | * @param nodeHandle_ - ROS node handle 78 | * @return true - in case of loading all params 79 | * @return false - in case of failure 80 | */ 81 | bool readRUNParmas(ros::NodeHandle& nodeHandle_); 82 | /** 83 | * @brief - detects features(contours in specific range of color) from inpout image 84 | * 85 | * @param img - input - image of front or back cameras 86 | * @return vector> - contours extracted from image 87 | */ 88 | vector> CropRowFeatures(camera& camera); 89 | /** 90 | * @brief main functoin to control the robot baesd on extracted features 91 | * 92 | * @param I_primary camera 93 | * @param I_secondary camera 94 | */ 95 | void Controller(camera& I_primary, camera& I_secondary); 96 | /** 97 | * @brief using simple regression fits line on extracted features from image 98 | * 99 | * @param img input image (draws the line on img) 100 | * @param ContourCenters 101 | * @return vector average line (the best fit) 102 | */ 103 | vector FitLineOnContures(Mat& img, vector& ContourCenters); 104 | /** 105 | * @brief estimates center of passed contours 106 | * 107 | * @param img is input image, contours will be printed on top of it 108 | * @param contours - extracted features from image 109 | * @return vector - center of passed contours 110 | */ 111 | vector getContureCenters(Mat& img, vector>& contours); 112 | /** 113 | * @brief fillters contours based on the fitting polygon/circle radius 114 | * 115 | * @param img input image 116 | * @param contours extracted contours 117 | * @return vector flitered contours 118 | */ 119 | vector filterContures(Mat& img, vector>& contours); 120 | /** 121 | * @brief computes control feature points, visualized on the image corners 122 | * 123 | * @param I primary camera containing features 124 | */ 125 | void compute_feature_point(camera& I); 126 | /** 127 | * @brief prints the features on the primary image 128 | * 129 | * @param I primary camera 130 | * @param Feature extracted features inside the neighbourhood 131 | * @param color of visualiation 132 | */ 133 | void draw_features(camera& I, Vector3f Feature, cv::Scalar color); 134 | /** 135 | * @brief switches primary and secondary cameras 136 | * 137 | * @param cam_primary id of current primary camera (front-> 1 back ->2) 138 | */ 139 | void switch_cameras(int& cam_primary); 140 | /** 141 | * @brief handles switching operation on the senario of crop row following 142 | * 143 | * @param I_primary camera 144 | * @param I_secondary camera 145 | * @param min_points minimum points inside the target image which wants to switch from 146 | */ 147 | void switching_controller(camera& I_primary, camera& I_secondary, unsigned int min_points); 148 | /** 149 | * @brief computes intersection of line passing through P,Q 150 | * with four different edges of the image 151 | * 152 | * @param P point in the bottom 153 | * @param Q point in the top 154 | * @return int returns index of the edge (top to bottom CW 0-3) 155 | */ 156 | int compute_intersection(Point2f& P, Point2f& Q); 157 | /** 158 | * @brief checkes for coordinates of feature to be inside the image 159 | * 160 | * @param R 161 | * @return MatrixXf 162 | */ 163 | Vector4i is_in_image(MatrixXf R); 164 | /** 165 | * @brief checkes for coordinates of feature to be inside the neighbourhood 166 | * 167 | * @param R 168 | * @return MatrixXf 169 | */ 170 | MatrixXf is_in_image_point(MatrixXf R); 171 | /** 172 | * @brief computes angle difference between 173 | * fitted line on P-Q and reference line on the middle 174 | * 175 | * @param P point in the bottom 176 | * @param Q point in the top 177 | * @return float 178 | */ 179 | float compute_Theta(Point2f& P, Point2f& Q); 180 | /** 181 | * @brief computes distance between two pointsets 182 | * 183 | * @param A 184 | * @param B 185 | * @return Vector2f distance of paires of A-B features 186 | */ 187 | Vector2f dist(MatrixXf& A, MatrixXf& B); 188 | /** 189 | * @brief warps the input angle to Pi 190 | * 191 | * @param angle input 192 | * @return float warped angle 193 | */ 194 | float wrapToPi(float angle); 195 | /** 196 | * @brief converts back point from homogenous coordinate 197 | * system to Euclidian coordinate system 198 | * 199 | * @param Mat 200 | * @return Vector2f 201 | */ 202 | Vector2f hom2euc(Vector3f Mat); 203 | /** 204 | * @brief gets the ids of featues with non-zero value in A 205 | * 206 | * @param A 207 | * @return VectorXi 208 | */ 209 | VectorXi find(Eigen::Vector4i A); 210 | /** 211 | * @brief shifts neighbourhood windows in passed camera 212 | * with input direction and valued laoded from params 213 | * 214 | * @param I passed camera 215 | * @param shift_dir direction of movement 216 | */ 217 | void shift_neighbourhood(camera& I, int shift_dir=1); 218 | /** 219 | * @brief used to initialize the neighbourhood windon the center of image 220 | * 221 | * @param I camera 222 | */ 223 | void initialize_neigbourhood(camera& I); 224 | /** 225 | * @brief updates points layed in the camera's neigbhourhood 226 | * 227 | * @param I input camera 228 | */ 229 | void is_in_neigbourhood(camera& I); 230 | /** 231 | * @brief prints neighbourhood on the image 232 | * 233 | * @param I input camera 234 | */ 235 | void draw_neighbourhood(camera& I); 236 | /** 237 | * @brief 238 | * 239 | * @param I 240 | * @return vector 241 | */ 242 | vector Compute_nh_ex(camera& I); 243 | /** 244 | * @brief projects point from camera to image coordinate system 245 | * 246 | * @param xc input image in camera coordinates 247 | * @return Point2f output image in image coordinate system 248 | */ 249 | Point2f camera2image(Point2f& xc); 250 | /** 251 | * @brief projects point from camera to image coordinate system 252 | * 253 | * @param xc input image in camera coordinates 254 | * @return Point2f output image in origin coordinate system 255 | */ 256 | Point2f camera2origin(Point2f& xc); 257 | /** 258 | * @brief projects point from origin to image coordinate system 259 | * 260 | * @param xc input image in origin coordinates 261 | * @return Point2f output image in image coordinate system 262 | */ 263 | Point2f origin2image(Point2f& xc); 264 | /** 265 | * @brief projects point from origin to camera coordinate system 266 | * 267 | * @param xc input origin in image coordinates 268 | * @return Point2f output image in camera coordinate system 269 | */ 270 | Point2f origin2camera(Point2f& xc); 271 | /** 272 | * @brief projects point from image to camera coordinate system 273 | * 274 | * @param xc input image in image coordinates 275 | * @return Point2f output image in camera coordinate system 276 | */ 277 | Point2f image2camera(Point2f& xc); 278 | /** 279 | * @brief projects point from image to origin coordinate system 280 | * 281 | * @param xc input image in image coordinates 282 | * @return Point2f output image in origin coordinate system 283 | */ 284 | Point2f image2origin(Point2f& xc); 285 | std::vector getEulerAngles(const nav_msgs::Odometry::ConstPtr& Pose); 286 | 287 | // This file defines the controller parameters 288 | camera front_cam; 289 | camera back_cam; 290 | 291 | int max_row_num; 292 | 293 | visual_crop_row_navigation::vs_msg VSMsg; 294 | 295 | double rho_b; 296 | double rho_f; 297 | double rho; 298 | double ty; 299 | double tz; 300 | double vf_des; 301 | double vb_des; 302 | 303 | double coef; 304 | int min_frame; 305 | 306 | int nh_L; 307 | int nh_H; 308 | 309 | int nh_offset; 310 | 311 | int mode; 312 | 313 | double w_max; 314 | double w_min; 315 | double z_min; 316 | 317 | int ex_Xc, ex_Yc; 318 | 319 | double lambda_x_1, lambda_x_2 ,lambda_x_3, lambda_x_4; 320 | double lambda_w_1, lambda_w_2 ,lambda_w_3, lambda_w_4; 321 | 322 | int steering_dir; 323 | bool drive_forward; 324 | int driving_dir; 325 | bool turning_mode; 326 | int height; 327 | int width; 328 | 329 | int navigation_dir; // Main navigation direction 1 = -> (L to R) , -1 = <- (R to L) 330 | int min_points_switch; // Minimum number of points to consider changing camera 331 | 332 | bool switching_controls_drive_forward; 333 | bool switching_controls_turning_mode; 334 | int switching_controls_steering_dir; 335 | int camera_ID; 336 | int maskTuneCamera; 337 | int fps; 338 | 339 | int minp_cnt; 340 | 341 | int id; 342 | Point2f P; 343 | Point2f Q; 344 | Vector3f F; 345 | Vector3f F_des; 346 | int side; 347 | 348 | bool publish_cmd_vel,publish_linear_vel; 349 | 350 | 351 | geometry_msgs::Twist VelocityMsg; 352 | sensor_msgs::Imu imu; 353 | vector RobotPose; 354 | vector RobotLinearVelocities; 355 | vector RobotAngularVelocities; 356 | vector RotationVec; 357 | vector TransVec; 358 | 359 | std::vector Vectlines; 360 | 361 | bool mask_tune; 362 | bool single_camera_mode; 363 | int FilterQuieSize; 364 | 365 | double Scale; 366 | 367 | float x,y; 368 | 369 | double LineDiffOffset; 370 | 371 | std::vector x_poses; 372 | std::vector y_poses; 373 | std::vector z_poses; 374 | 375 | Mat img_contour; 376 | 377 | int max_Hue; 378 | int min_Hue; 379 | int max_Saturation; 380 | int min_Saturation; 381 | int max_Value; 382 | int min_Value; 383 | 384 | double minContourSize; 385 | 386 | int center_min; 387 | int center_max; 388 | int center_min_off; 389 | int center_max_off; 390 | 391 | double max_vel_lin_; 392 | double min_vel_lin_; 393 | double max_incr_lin_; 394 | double k_p_lin_; 395 | double k_i_lin_; 396 | double k_d_lin_; 397 | 398 | double max_vel_ang_; 399 | double min_vel_ang_; 400 | double max_incr_ang_; 401 | double k_p_ang_; 402 | double k_i_ang_; 403 | double k_d_ang_; 404 | 405 | int LineFitting_method; 406 | 407 | private: 408 | 409 | #define d_t_ 1 / 20; 410 | double error_ang_, integral_ang_; 411 | double error_lin_, integral_lin_; 412 | 413 | }; 414 | } // namespace agribot_vs 415 | -------------------------------------------------------------------------------- /include/agribot_vs_nodehandler.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************************/ 2 | /* Paper: Visual-Servoing based Navigation for Monitoring Row-Crop Fields */ 3 | /* Alireza Ahmadi, Lorenzo Nardi, Nived Chebrolu, Chis McCool, Cyrill Stachniss */ 4 | /* All authors are with the University of Bonn, Germany */ 5 | /* maintainer: Alireza Ahmadi */ 6 | /* (Alireza.Ahmadi@uni-bonn.de / http://alirezaahmadi.xyz) */ 7 | /***************************************************************************************/ 8 | 9 | #pragma once 10 | 11 | #include "agribot_vs.h" 12 | #include 13 | 14 | using namespace cv; 15 | using namespace std; 16 | using namespace Eigen; 17 | 18 | namespace agribot_vs { 19 | /** 20 | * @brief node handler class of VisualServoing application 21 | * 22 | */ 23 | class AgribotVSNodeHandler { 24 | public: 25 | /** 26 | * @brief Construct a new Agribot V S Node Handler object 27 | * 28 | * @param node_handler 29 | */ 30 | AgribotVSNodeHandler(ros::NodeHandle& node_handler); 31 | /** 32 | * @brief Destroy the Agribot V S Node Handler object 33 | * 34 | */ 35 | virtual ~AgribotVSNodeHandler(); 36 | /** 37 | * @brief gets the input camera (primary one) to extract crop-rows and visual featues 38 | * 39 | * @param src is primary camera 40 | */ 41 | void CropRow_Tracking(camera& src); 42 | 43 | void imuCallBack(const sensor_msgs::Imu::ConstPtr& msg); 44 | /** 45 | * @brief gets front camera's image 46 | * 47 | * @param msg 48 | */ 49 | void imageFrontCalllBack(const sensor_msgs::ImageConstPtr& msg); 50 | /** 51 | * @brief gets rear camera's image 52 | * 53 | * @param msg 54 | */ 55 | void imageBackCalllBack(const sensor_msgs::ImageConstPtr& msg); 56 | /** 57 | * @brief gets the robot odometry from base controller 58 | * 59 | * @param msg 60 | */ 61 | void odomCallBack(const nav_msgs::Odometry::ConstPtr& msg); 62 | /** 63 | * @brief gets the poseof the robot in Lab from Mocap system 64 | * 65 | * @param msg 66 | */ 67 | void amclPoseCallBack(const geometry_msgs::PoseStamped& msg); 68 | /** 69 | * @brief stops the robot fror given time 70 | * 71 | * @param delay 72 | */ 73 | void StopForSec(float delay); 74 | /** 75 | * @brief publishes /cmd_vel topic to move the robot 76 | * 77 | * @param _in 78 | */ 79 | void publishVelocity(int _in=1); 80 | // void dynamicReconfig_callback(visual_crop_row_navigation::AgribotVSConfig &config, uint32_t level); 81 | 82 | ros::Publisher Time_pub; 83 | AgribotVS agribotVS; 84 | 85 | private: 86 | 87 | int state, in_state; 88 | 89 | // ROS node handle. 90 | ros::NodeHandle nodeHandle_; 91 | 92 | ros::Subscriber image_front_sub; 93 | ros::Subscriber image_back_sub; 94 | ros::Subscriber Mocap_sub; 95 | ros::Subscriber Odom_sub; 96 | ros::Subscriber IMU_sub; 97 | 98 | double mocap_roll, mocap_pitch, mocap_yaw; 99 | double imu_roll, imu_pitch, imu_yaw; 100 | ros::Publisher Log_pub; 101 | ros::Publisher VSVelocityPub; 102 | 103 | 104 | }; 105 | } // namespace agribot_vs 106 | -------------------------------------------------------------------------------- /launch/camera_umd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /launch/visualservoing.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /msg/vs_msg.msg: -------------------------------------------------------------------------------- 1 | float32 err_x 2 | float32 err_theta 3 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | visual_crop_row_navigation 4 | 0.0.0 5 | The Agribot Visual Servoing package 6 | 7 | Alireza 8 | 9 | TODO 10 | 11 | catkin 12 | 13 | dynamic_reconfigure 14 | roscpp 15 | message_generation 16 | message_runtime 17 | std_msgs 18 | cv_bridge 19 | sensor_msgs 20 | image_transport 21 | 22 | roscpp 23 | dynamic_reconfigure 24 | std_msgs 25 | geometry_msgs 26 | cv_bridge 27 | sensor_msgs 28 | image_transport 29 | message_runtime 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /params/agribot_vs_run.yaml: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------- 2 | #--------------------ROS node params-------------------- 3 | # controlling the velocity publisher stare (ON: True of OFF: false) 4 | publish_cmd_vel: true 5 | # controls publishing linear velocity (used fro debug and setting angular PID params) 6 | publish_linear_vel: true 7 | #------------------------------------------------------- 8 | #---------------------Runtime params-------------------- 9 | # Maxixmum number of rows to follow 10 | max_row_num: 20 11 | # Frame rate of ROs node = Fps 12 | fps: 20 13 | #------------------------------------------------------- 14 | #-------------------Application modes------------------- 15 | # The mode which is used for tuning the mask (no velocity will be published) 16 | mask_tune: true 17 | # In case of having only one camera set this param to true (the front camera will be used) 18 | single_camera_mode: true 19 | #------------------------------------------------------- 20 | #----------------Image ans Mask params------------------ 21 | # Camera used for mask tuninig 1: front, 2: rear 22 | maskTuneCamera: 1 23 | # Scale of the image processed (it resizes the input images before process) between 0.0 ~ 1.0 24 | Scale: 0.7 25 | # Min and Max of Hue Channel 26 | max_Hue: 80 27 | min_Hue: 40 28 | # Min and Max of Saturation Channel 29 | max_Saturation: 255 30 | min_Saturation: 50 31 | # Min and Max of Value Channel 32 | max_Value: 150 33 | min_value: 100 34 | #------------------------------------------------------- 35 | #------------------Feature space------------------------ 36 | # min radius of contour which will be detected and contribute on detecting line 37 | minContourSize: 2.0 38 | # ??? 39 | LineFitting_method: 1 40 | # ------------------------------------------------------- 41 | #---------------------- Neighbourhood--------------------- 42 | # Size of input image should be set here 43 | width: 640 44 | height: 480 45 | # default position of tracking windows 46 | ex_Xc: 320 47 | ex_Yc: 240 48 | # default size of tracking windows 49 | nh_L: 120 50 | nh_H: 250 51 | # ??? 52 | nh_offset: 200 53 | # min point where sate controller will switch the mode inside the tracking windows 54 | min_points_switch: 10 55 | # min point where sate controller will switch the mode inside image 56 | min_frame: 30 57 | # ??? 58 | coef: 55 59 | #------------------------------------------------------- 60 | #--------------------- Velocity Topic ------------------ 61 | # desired forward linear velocity 62 | vf_des: 0.2 63 | # desired backward linear velocity 64 | vb_des: 0.2 65 | # Max angular velocity 66 | w_max: 0.1 67 | # Min angular velocity 68 | w_min: 0.01 69 | # ??? 70 | z_min: 0.01 71 | # ------------------------------------------------------- 72 | #---------------------- Controller ---------------------- 73 | # position of camera w.r.t local origin 74 | ty: 0.6 75 | # height of camera from ground 76 | tz: 0.7 77 | # tilt angle of front camera 78 | rho_b: -60.0 79 | # tilt angle of rear camera 80 | rho_f: -60.0 81 | # ------------------------------------------------------- 82 | #---------------------- Gains --------------------------- 83 | # Gains of controll set for husky-clear path platform 84 | # gains for mode 1 85 | lambda_x_1: 10 86 | lambda_w_1: 1 87 | # gains for mode 2 88 | lambda_x_2: 0 89 | lambda_w_2: 5000 90 | # gains for mode 3 91 | lambda_x_3: 10 92 | lambda_w_3: 1 93 | # gains for mode 4 94 | lambda_x_4: 0 95 | lambda_w_4: 5000 96 | # ------------------------------------------------------- 97 | #---------------------- States to start ----------------- 98 | # uncomment each of these states and controller will start from that state to drive robot 99 | # camera_ID: 1 -> front Camera, camera_ID: 2 -> Rear Camera 100 | # drive_forward: true -> moving forward, drive_forward: false -> moving backward 101 | # turning_mode: based on the mode (state machine) is the robot is state of turning or not 102 | # steering_dir: 1 -> normal, steering_dir: -1 -> inverse (based on robots local coordinate system) 103 | # driving_dir: 1 -> moving forward, driving_dir: 2 moving backward 104 | 105 | mode: 1 106 | camera_ID: 1 107 | drive_forward: true 108 | turning_mode: false 109 | steering_dir: 1 110 | driving_dir: 1 111 | 112 | # mode: 2 113 | # camera_ID: 2 114 | # drive_forward: false 115 | # turning_mode: true 116 | # steering_dir: -1 117 | # driving_dir: 1 118 | 119 | # mode: 3 120 | # camera_ID: 2 121 | # drive_forward: true 122 | # turning_mode: false 123 | # steering_dir: -1 124 | # driving_dir: -1 125 | 126 | # mode: 4 127 | # camera_ID: 1 128 | # drive_forward: false 129 | # turning_mode: true 130 | # steering_dir: 1 131 | # driving_dir: -1 132 | 133 | 134 | -------------------------------------------------------------------------------- /src/agribot_vs.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************************/ 2 | /* Paper: Visual-Servoing based Navigation for Monitoring Row-Crop Fields */ 3 | /* Alireza Ahmadi, Lorenzo Nardi, Nived Chebrolu, Chis McCool, Cyrill Stachniss */ 4 | /* All authors are with the University of Bonn, Germany */ 5 | /* maintainer: Alireza Ahmadi */ 6 | /* (Alireza.Ahmadi@uni-bonn.de / http://alirezaahmadi.xyz) */ 7 | /***************************************************************************************/ 8 | 9 | #include "agribot_vs.h" 10 | 11 | using namespace cv; 12 | using namespace std; 13 | using namespace Eigen; 14 | 15 | using std::cerr; 16 | using cv::Mat; 17 | 18 | namespace agribot_vs{ 19 | 20 | AgribotVS::AgribotVS(){ 21 | 22 | RobotPose.resize(6, 0); 23 | RobotLinearVelocities.resize(3, 0); 24 | RobotAngularVelocities.resize(3, 0); 25 | RotationVec.resize(3, 0); 26 | TransVec.resize(3, 0); 27 | 28 | minp_cnt=0; 29 | 30 | id = 0; 31 | 32 | navigation_dir = 1; 33 | 34 | 35 | F_des << 0,0,0; 36 | F << 0,0,0; 37 | } 38 | AgribotVS::~AgribotVS(){ 39 | }; 40 | bool AgribotVS::readRUNParmas(ros::NodeHandle& nodeHandle_) { 41 | 42 | nodeHandle_.param("/agribot_vs/min_frame", min_frame, 15); 43 | 44 | nodeHandle_.param("/agribot_vs/coef", coef, 60.0); 45 | 46 | nodeHandle_.param("/agribot_vs/ex_Xc", ex_Xc, 640); 47 | nodeHandle_.param("/agribot_vs/ex_Yc", ex_Yc, 360); 48 | nodeHandle_.param("/agribot_vs/nh_offset", nh_offset, 200); 49 | nodeHandle_.param("/agribot_vs/nh_L", nh_L, 150); 50 | nodeHandle_.param("/agribot_vs/nh_H", nh_H, 250); 51 | 52 | nodeHandle_.param("/agribot_vs/minContourSize", minContourSize, 8.0); 53 | 54 | nodeHandle_.param("/agribot_vs/max_row_num", max_row_num, 80); 55 | 56 | nodeHandle_.param("/agribot_vs/mode", mode, 1); 57 | 58 | nodeHandle_.param("/agribot_vs/mask_tune", mask_tune, false); 59 | nodeHandle_.param("/agribot_vs/single_camera_mode", single_camera_mode, false); 60 | 61 | nodeHandle_.param("/agribot_vs/z_min", z_min, 0.15); 62 | 63 | nodeHandle_.param("/agribot_vs/w_min", w_min, 0.015); 64 | nodeHandle_.param("/agribot_vs/w_max", w_max, 0.15); 65 | nodeHandle_.param("/agribot_vs/vf_des", vf_des, 0.05); 66 | nodeHandle_.param("/agribot_vs/vb_des", vb_des, 0.05); 67 | 68 | nodeHandle_.param("/agribot_vs/ty", ty, 0.1); 69 | nodeHandle_.param("/agribot_vs/tz", tz, 1.034); 70 | 71 | nodeHandle_.param("/agribot_vs/rho_f", rho_f, -70.0); 72 | nodeHandle_.param("/agribot_vs/rho_b", rho_b, -40.0); 73 | 74 | nodeHandle_.param("/agribot_vs/camera_ID", camera_ID, 1); 75 | nodeHandle_.param("/agribot_vs/maskTuneCamera", maskTuneCamera, 1); 76 | nodeHandle_.param("/agribot_vs/fps", fps, 1); 77 | 78 | nodeHandle_.param("/agribot_vs/lambda_x_1", lambda_x_1, 1.0); 79 | nodeHandle_.param("/agribot_vs/lambda_w_1", lambda_w_1, 1.0); 80 | 81 | nodeHandle_.param("/agribot_vs/lambda_x_2", lambda_x_2, 1.0); 82 | nodeHandle_.param("/agribot_vs/lambda_w_2", lambda_w_2, 1.0); 83 | 84 | nodeHandle_.param("/agribot_vs/lambda_x_3", lambda_x_3, 1.0); 85 | nodeHandle_.param("/agribot_vs/lambda_w_3", lambda_w_3, 1.0); 86 | 87 | nodeHandle_.param("/agribot_vs/lambda_x_4", lambda_x_4, 1.0); 88 | nodeHandle_.param("/agribot_vs/lambda_w_4", lambda_w_4, 1.0); 89 | 90 | nodeHandle_.param("/agribot_vs/height", height, 720); 91 | nodeHandle_.param("/agribot_vs/width", width, 1280); 92 | 93 | nodeHandle_.param("/agribot_vs/drive_forward", drive_forward, true); 94 | nodeHandle_.param("/agribot_vs/turning_mode", turning_mode, false); 95 | nodeHandle_.param("/agribot_vs/steering_dir", steering_dir, 1); 96 | nodeHandle_.param("/agribot_vs/driving_dir", driving_dir, 1); 97 | 98 | nodeHandle_.param("/agribot_vs/min_points_switch", min_points_switch, 15); 99 | 100 | nodeHandle_.param("/agribot_vs/publish_cmd_vel", publish_cmd_vel, true); 101 | nodeHandle_.param("/agribot_vs/publish_linear_vel", publish_linear_vel, true); 102 | 103 | nodeHandle_.param("/agribot_vs/max_Hue", max_Hue, 80); 104 | nodeHandle_.param("/agribot_vs/min_Hue", min_Hue, 10); 105 | 106 | nodeHandle_.param("/agribot_vs/max_Saturation", max_Saturation, 255); 107 | nodeHandle_.param("/agribot_vs/min_Saturation", min_Saturation, 100); 108 | 109 | nodeHandle_.param("/agribot_vs/max_Value", max_Value, 255); 110 | nodeHandle_.param("/agribot_vs/min_Value", min_Value, 100); 111 | 112 | nodeHandle_.param("/agribot_vs/Scale", Scale, 0.5); 113 | 114 | cout << "Run parameters loading ..." << endl; 115 | 116 | rho_b *=DEG2RAD; 117 | rho_f *=DEG2RAD; 118 | 119 | return true; 120 | } 121 | vector> AgribotVS::CropRowFeatures(camera& camera) { 122 | Mat img = camera.image; 123 | // // convert to HSV color space 124 | cv::Mat hsvImage; 125 | cv::cvtColor(img, hsvImage, CV_BGR2HSV); 126 | // split the channels 127 | std::vector hsvChannels; 128 | cv::split(hsvImage, hsvChannels); 129 | 130 | // is the color within the lower hue range? 131 | cv::Mat hueMask; 132 | cv::Mat hueImg = hsvChannels[0]; 133 | cv::inRange(hueImg, min_Hue, max_Hue, hueMask); 134 | cv::Mat saturationMask; 135 | cv::Mat saturationImg = hsvChannels[1]; 136 | cv::inRange(saturationImg, min_Saturation, max_Saturation, saturationMask); 137 | cv::Mat valueMask; 138 | cv::Mat valueImg = hsvChannels[2]; 139 | cv::inRange(valueImg, min_Value, max_Value, valueMask); 140 | 141 | hueMask = hueMask & saturationMask & valueMask; 142 | 143 | vector> contours; 144 | vector hierarchy; 145 | RNG rng(12345); 146 | 147 | // finds cluster/contour in the image 148 | findContours(hueMask, contours, hierarchy, CV_RETR_TREE, 149 | CV_CHAIN_APPROX_SIMPLE, Point(0, 0)); 150 | 151 | // Draw contours 152 | img_contour = Mat::zeros(hueMask.size(), CV_8UC3); 153 | Scalar color = Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)); 154 | for(size_t i = 0; i < contours.size(); i++){ 155 | drawContours(img_contour, contours, i, color, 1, 8, hierarchy, 0, Point()); 156 | } 157 | 158 | if(mask_tune && maskTuneCamera == camera_ID){ 159 | Mat Comb_HSV; 160 | hconcat(hueMask,saturationMask,Comb_HSV); 161 | hconcat(Comb_HSV,valueMask ,Comb_HSV); 162 | cv::resize(Comb_HSV, Comb_HSV, cv::Size(), Scale, Scale); 163 | cv::imshow("HSV images (Left: H, Middle: S, Right: V)", Comb_HSV); 164 | 165 | cv::resize(img_contour, img_contour, cv::Size(), Scale, Scale); 166 | imshow("detected Contours", img_contour); 167 | waitKey(1); 168 | } 169 | return contours; 170 | } 171 | vector AgribotVS::getContureCenters(Mat& img, vector>& contours){ 172 | /// Approximate contours to polygons + get bounding rects and circles 173 | vector> contours_poly(contours.size()); 174 | vector center(contours.size()); 175 | vector radius(contours.size()); 176 | 177 | // find enclosing Polygon whcih fits arround the contures 178 | for (size_t i = 0; i < contours.size(); i++) { 179 | approxPolyDP(Mat(contours[i]), contours_poly[i], 2, true); 180 | minEnclosingCircle((Mat)contours_poly[i], center[i], radius[i]); 181 | cv::circle(img, Point(center[i].x, center[i].y),3, Scalar(51, 204, 51),CV_FILLED, 8,0); 182 | } 183 | return center; 184 | } 185 | vector AgribotVS::filterContures(Mat& img, vector>& contours){ 186 | /// Approximate contours to polygons + get bounding rects and circles 187 | vector> contours_poly(contours.size()); 188 | vector center(contours.size()); 189 | vector radius(contours.size()); 190 | vector Filtered_Centers; 191 | RNG rng(12345); 192 | // find enclosing Polygon whcih fits arround the contures 193 | for (size_t i = 0; i < contours.size(); i++) { 194 | approxPolyDP(Mat(contours[i]), contours_poly[i], 1, true); 195 | minEnclosingCircle((Mat)contours_poly[i], center[i], radius[i]); 196 | if (center[i].x >= (img.cols/2) - center_min_off && 197 | center[i].x <= (img.cols/2) + center_max_off && 198 | radius[i] >= this->minContourSize) { 199 | Filtered_Centers.push_back(center[i]); 200 | } 201 | 202 | } 203 | return Filtered_Centers; 204 | } 205 | vector AgribotVS::FitLineOnContures(Mat& img, vector& ContourCenters){ 206 | 207 | 208 | Point2f P1; 209 | Point2f P2; 210 | vector AvgLine; 211 | AvgLine.resize(1); 212 | 213 | if(ContourCenters.size() > 0 ){ 214 | Vec4f linefit; 215 | cv::fitLine(ContourCenters,linefit,CV_DIST_L2,0,0.01,0.01); 216 | 217 | P1.x = linefit[0] + linefit[2]; 218 | P1.y = linefit[1] + linefit[3]; 219 | P2.x = linefit[2]; 220 | P2.y = linefit[3]; 221 | 222 | Vector3f top_left(0,0,1); 223 | Vector3f top_right(width,0,1); 224 | Vector3f bottom_left(0,height,1); 225 | Vector3f bottom_right(width,height,1); 226 | 227 | // compute image border lines 228 | Vector3f l_ib = bottom_left.cross(bottom_right); 229 | Vector3f l_it = top_left.cross(top_right); 230 | Vector3f l_il = top_left.cross(bottom_left); 231 | Vector3f l_ir = top_right.cross(bottom_right); 232 | 233 | // compute line PQ 234 | Vector3f P_h(P1.x,P1.y,1); 235 | Vector3f Q_h(P2.x,P2.y,1); 236 | Vector3f l = P_h.cross(Q_h); 237 | 238 | // compute intersections with all four lines 239 | Vector2f R_t = hom2euc(l.cross(l_it)); 240 | Vector2f R_l = hom2euc(l.cross(l_il)); 241 | Vector2f R_b = hom2euc(l.cross(l_ib)); 242 | Vector2f R_r = hom2euc(l.cross(l_ir)); 243 | 244 | // compute points within the image 245 | MatrixXf R(4,2); 246 | R << R_t(0), R_t(1), 247 | R_l(0), R_l(1), 248 | R_b(0), R_b(1), 249 | R_r(0), R_r(1); 250 | 251 | MatrixXf R_out(2,2); 252 | R_out = is_in_image_point(R); 253 | cv::line(img, Point(R_out(0,0), R_out(0,1)),Point(R_out(1,0), R_out(1,1)), Scalar(0, 0, 255), 1, CV_AA); 254 | AvgLine[0][0] =R_out(0,0); 255 | AvgLine[0][1] =R_out(0,1); 256 | AvgLine[0][2] =R_out(1,0); 257 | AvgLine[0][3] =R_out(1,1); 258 | } 259 | return AvgLine; 260 | } 261 | //************************************************************************************************* 262 | void AgribotVS::Controller(camera& I_primary, camera& I_secondary){ 263 | // id = 1 => Row controller 264 | // id = 2 => Column controller 265 | // input feature 266 | 267 | float X = F(0); 268 | float Y = F(1); 269 | float Theta = F(2); 270 | 271 | float v = 0; 272 | if( driving_dir ==1){ 273 | v = driving_dir*vf_des; 274 | }else{ 275 | v = driving_dir*vb_des; 276 | } 277 | // compute interaction matrix 278 | 279 | MatrixXf lambda(2,1); 280 | if(driving_dir == 1){ 281 | if(steering_dir == 1){ // mode 1 282 | lambda << lambda_x_1,lambda_w_1; 283 | rho = rho_f; 284 | mode = 1; 285 | //ty = 0.0; 286 | }else{ // mode 2 287 | lambda << lambda_x_2,lambda_w_2; 288 | rho = rho_b; 289 | mode = 2; 290 | //ty = 1.08; 291 | } 292 | }else{ 293 | if(steering_dir == -1){ // mode 3 294 | lambda << lambda_x_3,lambda_w_3; 295 | rho = rho_b; 296 | mode = 3; 297 | //ty = 1.08; 298 | }else{ // mode 4 299 | lambda << lambda_x_4,lambda_w_4; 300 | rho = rho_f; 301 | mode = 4; 302 | //ty = 0.0; 303 | } 304 | } 305 | 306 | MatrixXf Ls(3,6); 307 | Ls << -(sin(rho)+Y*cos(rho))/tz, 0, X*(sin(rho)+Y*cos(rho))/tz, X*Y, -1-pow(X,2), Y, 308 | 0, -(sin(rho)+Y*cos(rho))/tz, Y*(sin(rho)+Y*cos(rho))/tz, 1+pow(Y,2), -X*Y, -X, 309 | cos(rho)*pow(cos(Theta),2)/tz, cos(rho)*cos(Theta)*sin(Theta)/tz, -(cos(rho)*cos(Theta)*(Y*sin(Theta) + X*cos(Theta)))/tz, -(Y*sin(Theta) + X*cos(Theta))*cos(Theta), -(Y*sin(Theta) + X*cos(Theta))*sin(Theta), -1; 310 | 311 | // compute tranformation between robot to camera frame 312 | MatrixXf c(2,6); 313 | if( camera_ID == 1){ 314 | c << 0, -sin(rho), cos(rho), 0, 0, 0, 315 | -ty, 0, 0, 0, -cos(rho ), -sin(rho); 316 | }else{ 317 | c << 0, sin(rho), -cos(rho), 0, 0, 0, 318 | -ty, 0, 0, 0, cos(rho), sin(rho); 319 | } 320 | 321 | MatrixXf cTR(6,2); 322 | cTR = c.transpose(); 323 | 324 | MatrixXf Tv(6,1); 325 | Tv = cTR.col(0); 326 | 327 | MatrixXf Tw(6,1); 328 | Tw = cTR.col(1); 329 | 330 | // compute Jacobian 331 | MatrixXf Jv(2,6); 332 | Jv << Ls.row(id), 333 | Ls.row(2); 334 | Jv = Jv*Tv; 335 | 336 | MatrixXf Jw(2,6); 337 | Jw << Ls.row(id), 338 | Ls.row(2); 339 | Jw = Jw*Tw; 340 | 341 | // // compute control law 342 | Vector2f err((F[id] - F_des[id]), wrapToPi(F[2] - F_des[2])); 343 | 344 | // set weights 345 | MatrixXf tmp_lambda(2,1); 346 | tmp_lambda << lambda(0)*err(0), 347 | lambda(1)*err(1); 348 | 349 | // compute control 350 | MatrixXf Jw_pinv(6,2); 351 | Jw_pinv = Jw.completeOrthogonalDecomposition().pseudoInverse(); 352 | 353 | MatrixXf w = -Jw_pinv*(tmp_lambda + Jv*v); 354 | 355 | w(0,0) = copysign(min(abs(w(0,0)),(float)w_max), w(0,0)); 356 | 357 | if(abs(v) < 0.05 || (I_primary.nh_points.size() < 5 && I_secondary.nh_points.size() <5)){ 358 | VelocityMsg.angular.z = 0; 359 | }else{ 360 | if(w(0,0) > 0.3) w(0,0) = 0.3; 361 | VelocityMsg.angular.z = steering_dir * w(0,0); 362 | } 363 | 364 | if(abs(VelocityMsg.angular.z) < z_min || mode == 4 || mode==2)VelocityMsg.angular.z=0.0; 365 | VelocityMsg.linear.x = v; 366 | 367 | VSMsg.err_x=abs(err(0)); 368 | VSMsg.err_theta=abs(err(1)); 369 | } 370 | void AgribotVS::compute_feature_point(camera& I){ 371 | 372 | if(drive_forward){ 373 | P.x = I.lines[0][0]; 374 | P.y = I.lines[0][1]; 375 | Q.x = I.lines[0][2]; 376 | Q.y = I.lines[0][3]; 377 | }else{ 378 | P.x = I.lines[0][2]; 379 | P.y = I.lines[0][3]; 380 | Q.x = I.lines[0][0]; 381 | Q.y = I.lines[0][1]; 382 | } 383 | 384 | I.nh_ex = Compute_nh_ex(I); 385 | I.nh.Xc = (I.nh_ex[0].x + I.nh_ex[1].x)/2; 386 | I.nh.Yc = (I.nh_ex[0].y + I.nh_ex[1].y)/2; 387 | 388 | // computes intersection side 389 | cv::circle(I.image, Point(P.x, P.y),8, Scalar(0,0,255),CV_FILLED, 12,0); 390 | cv::circle(I.image, Point(Q.x, Q.y),8, Scalar(0,255,255),CV_FILLED, 12,0); 391 | 392 | // compute Theta 393 | float Theta = compute_Theta(P,Q); 394 | 395 | // compute F 396 | Point2f _F = camera2image(P); 397 | F << _F.x, 398 | _F.y, 399 | Theta; 400 | F_des << 0, 401 | height/2, 402 | 0; 403 | id = 0; 404 | } 405 | void AgribotVS::switching_controller(camera& I_primary, camera& I_secondary, unsigned int min_points){ 406 | 407 | is_in_neigbourhood(I_primary); 408 | 409 | double avg_nh_points_y = 0.0; 410 | for (unsigned int i = 0; i < I_primary.nh_points.size(); ++i){ 411 | avg_nh_points_y += I_primary.nh_points[i].y; 412 | } 413 | avg_nh_points_y /= I_primary.nh_points.size(); 414 | 415 | double avg_points_y =0.0; 416 | for (unsigned int i = 0; i < I_primary.points.size(); ++i){ 417 | avg_points_y += I_primary.points[i].y; 418 | } 419 | avg_points_y /= I_primary.points.size(); 420 | 421 | if(I_primary.nh_points.size() < min_points){ 422 | minp_cnt++; 423 | }else{ 424 | minp_cnt = 0; 425 | } 426 | 427 | if((I_primary.nh_points.size() == 0 && minp_cnt >=min_frame) || 428 | (avg_nh_points_y < (double)coef && avg_points_y < (double)coef*2) || 429 | (avg_nh_points_y > (double)height-coef && avg_points_y > (double)height-coef*2)){ 430 | minp_cnt=0; 431 | cout << "I_primary doesn't see anything !!!! ID: " << camera_ID << endl; 432 | if(I_secondary.points.size() < min_points){ 433 | cout << "turning_mode: " << turning_mode << endl; 434 | // No points visible in both cameras 435 | if(turning_mode){ 436 | // 2->3 and 4->1 437 | mode++; 438 | if(mode == 5)mode=1; 439 | cout << "TURNING..." << endl; 440 | // switch steering direction 441 | driving_dir = -driving_dir; 442 | // switch behavior cameras 443 | drive_forward = true; 444 | turning_mode = false; 445 | cout << "turning mode OFF" << endl; 446 | shift_neighbourhood(I_primary, steering_dir); 447 | is_in_neigbourhood(I_primary); 448 | 449 | } 450 | }else{ 451 | mode++; 452 | // 1->2 and 3->4 453 | cout << "SWITCHING CAMERAS" << endl; 454 | switch_cameras(camera_ID); 455 | initialize_neigbourhood(I_secondary); 456 | initialize_neigbourhood(I_primary); 457 | is_in_neigbourhood(I_primary); 458 | turning_mode = true; 459 | drive_forward = false; 460 | steering_dir = -steering_dir; 461 | cout << "turning mode ON" << endl; 462 | } 463 | }else{ 464 | cout << 465 | "mode: " << mode << 466 | ", cam: " << camera_ID << 467 | ", df: " << drive_forward << 468 | ", sd: " << steering_dir << 469 | ", dd: " << driving_dir << 470 | ", nh_p: " << I_primary.nh_points.size() << 471 | endl; 472 | } 473 | } 474 | //************************************************************************************************* 475 | int AgribotVS::compute_intersection(Point2f& P, Point2f& Q){ 476 | Vector3f top_left(0,0,1); 477 | Vector3f top_right(width,0,1); 478 | Vector3f bottom_left(0,height,1); 479 | Vector3f bottom_right(width,height,1); 480 | 481 | // compute image border lines 482 | Vector3f l_ib = bottom_left.cross(bottom_right); 483 | Vector3f l_it = top_left.cross(top_right); 484 | Vector3f l_il = top_left.cross(bottom_left); 485 | Vector3f l_ir = top_right.cross(bottom_right); 486 | 487 | // compute line PQ 488 | Vector3f P_h(P.x,P.y,1); 489 | Vector3f Q_h(Q.x,Q.y,1); 490 | Vector3f l = P_h.cross(Q_h); 491 | 492 | // compute intersections with all four lines 493 | Vector2f R_t = hom2euc(l.cross(l_it)); 494 | Vector2f R_l = hom2euc(l.cross(l_il)); 495 | Vector2f R_b = hom2euc(l.cross(l_ib)); 496 | Vector2f R_r = hom2euc(l.cross(l_ir)); 497 | 498 | // compute points within the image 499 | MatrixXf R(4,2); 500 | R << R_t(0), R_t(1), 501 | R_l(0), R_l(1), 502 | R_b(0), R_b(1), 503 | R_r(0), R_r(1); 504 | Vector4i in = is_in_image(R); 505 | 506 | int ind_min = 0; 507 | int ind_max = 0; 508 | int tmp_min_y = 10000; 509 | int tmp_max_y = 0; 510 | for(int i = 0; i < 4; i++){ 511 | if(in(i) == 1){ 512 | if(R(i,1) < tmp_min_y){ 513 | tmp_min_y = R(i,1); 514 | ind_min = i; 515 | } 516 | if(R(i,1) > tmp_max_y){ 517 | tmp_max_y = R(i,1); 518 | ind_max = i; 519 | } 520 | } 521 | } 522 | 523 | Q.x = R(ind_min,0); 524 | Q.y = R(ind_min,1); 525 | P.x = R(ind_max,0); 526 | P.y = R(ind_max,1); 527 | 528 | return ind_max; 529 | } 530 | //************************************************************************************************* 531 | Vector2f AgribotVS::hom2euc(Vector3f Mat){ 532 | Vector2f _Mat; 533 | _Mat << Mat(0,0)/Mat(2,0), 534 | Mat(1,0)/Mat(2,0); 535 | return _Mat; 536 | } 537 | VectorXi AgribotVS::find(Eigen::Vector4i A){ 538 | VectorXi idxs; 539 | for(int i=0; i= 0 && R(i,0) <= width && R(i,1) >= 0 && R(i,1) <= height){ 548 | _In(i) = 1; 549 | } 550 | } 551 | return _In; 552 | } 553 | MatrixXf AgribotVS::is_in_image_point(MatrixXf R){ 554 | MatrixXf out_p(2,2); 555 | int cnt = 0; 556 | for(int i = 0; i < 4; i++){ 557 | if(R(i,0) >= 0 && R(i,0) <= width && R(i,1) >= 0 && R(i,1) <= height){ 558 | out_p(cnt,0) = R(i,0); 559 | out_p(cnt,1) = R(i,1); 560 | cnt++; 561 | } 562 | } 563 | return out_p; 564 | } 565 | //************************************************************************************************* 566 | Vector2f AgribotVS::dist(MatrixXf& A, MatrixXf& B){ 567 | Vector2f dis; 568 | Vector2f tmp; 569 | //D = sqrt(sum((A-B).^2,2)); 570 | for(unsigned int i = 0; i < A.cols(); i++){ 571 | tmp << sqrt(pow((A(0,i) - B(0,0)),2)), 572 | sqrt(pow((A(1,i) - B(0,0)),2)); 573 | dis << tmp; 574 | } 575 | return dis; 576 | } 577 | void AgribotVS::switch_cameras(int& cam_primary){ 578 | if(cam_primary == 1) 579 | cam_primary=2; 580 | else{ 581 | cam_primary =1; 582 | } 583 | } 584 | //************************************************************************************************* 585 | Point2f AgribotVS::camera2image(Point2f& xc){ 586 | Point2f xi; 587 | xi.x = xc.x - width/2; 588 | xi.y = xc.y - height/2; 589 | return xi; 590 | } 591 | Point2f AgribotVS::camera2origin(Point2f& xc){ 592 | Point2f xi; 593 | xi.x = xc.x; 594 | xi.y = height - xc.y; 595 | return xi; 596 | } 597 | Point2f AgribotVS::origin2camera(Point2f& xc){ 598 | Point2f xi; 599 | xi.x = xc.x; 600 | xi.y = height - xc.y; 601 | return xi; 602 | } 603 | Point2f AgribotVS::origin2image(Point2f& xc){ 604 | Point2f xi,xo; 605 | xi = origin2camera(xc); 606 | xo = camera2origin(xi); 607 | return xo; 608 | } 609 | Point2f AgribotVS::image2camera(Point2f& xc){ 610 | Point2f xi; 611 | xi.x = xc.x + width/2; 612 | xi.y = xc.y + height/2; 613 | return xi; 614 | } 615 | Point2f AgribotVS::image2origin(Point2f& xc){ 616 | Point2f xi,xo; 617 | xi = image2camera(xc); 618 | xo = camera2origin(xi); 619 | return xo; 620 | } 621 | //************************************************************************************************* 622 | void AgribotVS::initialize_neigbourhood(camera& I){ 623 | cout << ex_Xc << " " << ex_Yc << endl; 624 | I.nh.Xc = ex_Xc; 625 | I.nh.Yc = ex_Yc; 626 | I.nh.L = nh_L; 627 | I.nh.H = nh_H; 628 | I.nh.offset = nh_offset; 629 | I.nh.shift = navigation_dir * I.nh.offset; 630 | cout << "***********initialize_neigbourhood***********" << endl; 631 | } 632 | void AgribotVS::shift_neighbourhood(camera& I, int shift_dir){ 633 | // To Do: 634 | // compute using proper formula later 635 | int dX = shift_dir*I.nh.shift; 636 | cout << "###########shift_neighbourhood############" << endl; 637 | // new neighbourhood to select row 638 | cout << I.nh.Xc << " " << I.nh.Xc + dX << " " << I.nh.shift << " " << shift_dir << endl; 639 | I.nh.Xc = I.nh.Xc + dX; 640 | } 641 | void AgribotVS::is_in_neigbourhood(camera& I){ 642 | I.nh_points.clear(); 643 | for(size_t i = 0; i < I.points.size(); i++){ 644 | if(I.points[i].x > I.nh.Xc - I.nh.L/2 && I.points[i].x < I.nh.Xc + I.nh.L/2 645 | && I.points[i].y > I.nh.Yc -I.nh.H/2 && I.points[i].y < I.nh.Yc + I.nh.H/2){ 646 | I.nh_points.push_back(I.points[i]); 647 | } 648 | } 649 | } 650 | void AgribotVS::draw_neighbourhood(camera& I){ 651 | Point2f P(I.nh.Xc,I.nh.Yc); 652 | int X = P.x - I.nh.L/2; 653 | int Y = P.y - I.nh.H/2; 654 | Rect RectangleToDraw3(X, Y, I.nh.L, I.nh.H); 655 | rectangle(I.image, RectangleToDraw3, Scalar(255, 204, 102), 3, 8, 0); 656 | } 657 | //************************************************************************************************* 658 | void AgribotVS::draw_features(camera& I, Vector3f Feature, cv::Scalar color){ 659 | 660 | Point2f P_1(Feature(0),Feature(1)); 661 | P_1 = image2camera(P_1); 662 | Point2f P_2(P_1.x + 100*cos(Feature(2) - M_PI/2), 663 | P_1.y + 100*sin(Feature(2) - M_PI/2)); 664 | arrowedLine(I.image, P_1, P_2, color,3); 665 | } 666 | vector AgribotVS::Compute_nh_ex(camera& I){ 667 | vector nh_ex; 668 | nh_ex.resize(2); 669 | int tmp_min_y = height; 670 | int tmp_max_y = 0; 671 | 672 | for(size_t i = 0; i < I.nh_points.size(); i++) 673 | { 674 | if(I.nh_points[i].y < tmp_min_y){ 675 | tmp_min_y = I.nh_points[i].y; 676 | nh_ex[0] = I.nh_points[i]; 677 | } 678 | if(I.nh_points[i].y > tmp_max_y){ 679 | tmp_max_y = I.nh_points[i].y; 680 | nh_ex[1] = I.nh_points[i]; 681 | } 682 | } 683 | return nh_ex; 684 | } 685 | float AgribotVS::compute_Theta(Point2f& P, Point2f& Q){ 686 | // compute phi 687 | float Y = -Q.y+P.y; 688 | float X = Q.x-P.x; 689 | float phi = atan2(Y,X); 690 | 691 | // compute Theta 692 | float Theta = wrapToPi(M_PI/2 - phi); 693 | 694 | return Theta; 695 | } 696 | float AgribotVS::wrapToPi(float angle){ 697 | while(angle < -M_PI && angle > M_PI){ 698 | if(angle > M_PI){ 699 | angle = angle - 2*M_PI; 700 | }else if(angle < -M_PI){ 701 | angle = angle + 2*M_PI; 702 | } 703 | } 704 | return angle; 705 | } 706 | std::vector AgribotVS::getEulerAngles(const nav_msgs::Odometry::ConstPtr& Pose) { 707 | std::vector EulerAngles; 708 | EulerAngles.resize(3, 0); 709 | tf::Quaternion q(Pose->pose.pose.orientation.x, Pose->pose.pose.orientation.y, 710 | Pose->pose.pose.orientation.z, 711 | Pose->pose.pose.orientation.w); 712 | tf::Matrix3x3 m(q); 713 | m.getRPY(EulerAngles[0], EulerAngles[1], EulerAngles[2]); 714 | return EulerAngles; 715 | } 716 | } // namespace agribot_vs 717 | -------------------------------------------------------------------------------- /src/agribot_vs_node.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************************/ 2 | /* Paper: Visual-Servoing based Navigation for Monitoring Row-Crop Fields */ 3 | /* Alireza Ahmadi, Lorenzo Nardi, Nived Chebrolu, Chis McCool, Cyrill Stachniss */ 4 | /* All authors are with the University of Bonn, Germany */ 5 | /* maintainer: Alireza Ahmadi */ 6 | /* (Alireza.Ahmadi@uni-bonn.de / http://alirezaahmadi.xyz) */ 7 | /***************************************************************************************/ 8 | #include 9 | #include "agribot_vs_nodehandler.h" 10 | #include "agribot_vs.h" 11 | 12 | #include "std_msgs/String.h" 13 | #include 14 | #include 15 | 16 | int main(int argc, char** argv) { 17 | // initialize node 18 | ros::init(argc, argv, "agribot_vs"); 19 | 20 | // node handler 21 | ros::NodeHandle nodeHandle; 22 | agribot_vs::AgribotVSNodeHandler vs_NodeH(nodeHandle); 23 | ros::Rate loop_rate(vs_NodeH.agribotVS.fps); 24 | 25 | if(!vs_NodeH.agribotVS.mask_tune)cout << "Mask Tune Mode ..." << endl; 26 | if(vs_NodeH.agribotVS.single_camera_mode)cout << "Single Camera Mode (Front Camera will only be used..)" << endl; 27 | 28 | agribot_vs::camera *I_primary,*I_secondary; 29 | if(vs_NodeH.agribotVS.camera_ID == 1){ 30 | I_primary = &vs_NodeH.agribotVS.front_cam; 31 | I_secondary = &vs_NodeH.agribotVS.back_cam; 32 | }else { 33 | I_primary = &vs_NodeH.agribotVS.back_cam; 34 | I_secondary = &vs_NodeH.agribotVS.front_cam; 35 | } 36 | vs_NodeH.agribotVS.initialize_neigbourhood(*I_primary); 37 | vs_NodeH.agribotVS.initialize_neigbourhood(*I_secondary); 38 | int cnt =0; 39 | 40 | while(ros::ok()){ 41 | 42 | if(cnt < vs_NodeH.agribotVS.max_row_num){ 43 | 44 | if(vs_NodeH.agribotVS.single_camera_mode){ 45 | I_secondary = I_primary; 46 | } 47 | if(!vs_NodeH.agribotVS.mask_tune){ 48 | vs_NodeH.agribotVS.switching_controller(*I_primary, *I_secondary, vs_NodeH.agribotVS.min_points_switch); 49 | 50 | if(vs_NodeH.agribotVS.camera_ID == 1){ 51 | I_primary = &vs_NodeH.agribotVS.front_cam; 52 | I_secondary = &vs_NodeH.agribotVS.back_cam; 53 | }else { 54 | I_primary = &vs_NodeH.agribotVS.back_cam; 55 | I_secondary = &vs_NodeH.agribotVS.front_cam; 56 | } 57 | 58 | vs_NodeH.agribotVS.compute_feature_point(*I_primary); 59 | vs_NodeH.agribotVS.Controller(*I_primary,*I_secondary); 60 | 61 | if(!I_primary->image.empty()){ 62 | vs_NodeH.agribotVS.draw_neighbourhood(*I_primary); 63 | vs_NodeH.agribotVS.draw_features(*I_primary, vs_NodeH.agribotVS.F_des, cv::Scalar(0, 255, 0)); 64 | vs_NodeH.agribotVS.draw_features(*I_primary, vs_NodeH.agribotVS.F, cv::Scalar(0, 0, 255)); 65 | 66 | // draw plant centers in image (in neighbourhood) 67 | for(size_t i = 0; i < I_primary->nh_points.size(); i++){ 68 | cv::circle(I_primary->image, Point(I_primary->nh_points[i].x,I_primary->nh_points[i].y),5, Scalar(0, 204, 255), CV_FILLED, 8,0); 69 | } 70 | 71 | Mat des_comp; 72 | cv::resize(I_primary->image, des_comp, cv::Size(), vs_NodeH.agribotVS.Scale, vs_NodeH.agribotVS.Scale); 73 | imshow("Cameras", des_comp); 74 | waitKey(1); 75 | }else{ 76 | cout << "Is Image empty? Camera-Primary: " << I_primary->image.empty() << " , Camera-Secondary "<< I_secondary->image.empty() << endl; 77 | } 78 | } 79 | 80 | if(I_primary->points.size() == 0){ 81 | vs_NodeH.publishVelocity(0); 82 | }else{ 83 | vs_NodeH.publishVelocity(); 84 | } 85 | 86 | ros::Time curr_time = ros::Time::now(); 87 | rosgraph_msgs::Clock curr_time_msg; 88 | curr_time_msg.clock = curr_time; 89 | vs_NodeH.Time_pub.publish(curr_time_msg); 90 | } 91 | 92 | if(cnt < 1000)cnt++; 93 | ros::spinOnce(); 94 | loop_rate.sleep(); 95 | } 96 | 97 | return 0; 98 | } 99 | -------------------------------------------------------------------------------- /src/agribot_vs_nodehandler.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************************/ 2 | /* Paper: Visual-Servoing based Navigation for Monitoring Row-Crop Fields */ 3 | /* Alireza Ahmadi, Lorenzo Nardi, Nived Chebrolu, Chis McCool, Cyrill Stachniss */ 4 | /* All authors are with the University of Bonn, Germany */ 5 | /* maintainer: Alireza Ahmadi */ 6 | /* (Alireza.Ahmadi@uni-bonn.de / http://alirezaahmadi.xyz) */ 7 | /***************************************************************************************/ 8 | 9 | #include "agribot_vs_nodehandler.h" 10 | #include "agribot_vs.h" 11 | #include 12 | 13 | namespace agribot_vs { 14 | 15 | AgribotVSNodeHandler::AgribotVSNodeHandler(ros::NodeHandle& nodeHandle): nodeHandle_(nodeHandle){ 16 | ROS_ERROR("Visual Servoing core is running..."); 17 | if (!agribotVS.readRUNParmas(nodeHandle_)) { 18 | ROS_ERROR("Could not read parameters."); 19 | ros::requestShutdown(); 20 | } 21 | 22 | // Subscribers 23 | image_front_sub = nodeHandle_.subscribe("/front/rgb/image_raw", 2, &AgribotVSNodeHandler::imageFrontCalllBack,this); 24 | image_back_sub = nodeHandle_.subscribe("/back/rgb/image_raw", 2, &AgribotVSNodeHandler::imageBackCalllBack,this); 25 | Mocap_sub = nodeHandle_.subscribe("/amcl_pose", 1, &AgribotVSNodeHandler::amclPoseCallBack,this); 26 | Odom_sub = nodeHandle_.subscribe("/odometry/raw", 10, &AgribotVSNodeHandler::odomCallBack,this); 27 | IMU_sub = nodeHandle_.subscribe("/imu/data", 1000, &AgribotVSNodeHandler::imuCallBack,this); 28 | 29 | // Publishers 30 | Time_pub = nodeHandle_.advertise("/clock", 10); 31 | VSVelocityPub = nodeHandle_.advertise("/cmd_vel", 10); 32 | Log_pub = nodeHandle_.advertise("/vs_msg", 10); 33 | 34 | agribotVS.VelocityMsg.linear.x =0.0; 35 | agribotVS.VelocityMsg.angular.z =0.0; 36 | 37 | state = 0; 38 | in_state = 0; 39 | } 40 | AgribotVSNodeHandler::~AgribotVSNodeHandler() { 41 | } 42 | void AgribotVSNodeHandler::CropRow_Tracking(camera& src){ 43 | // finding contour from image baed on crops in rows 44 | src.contours = agribotVS.CropRowFeatures(src); 45 | if(!agribotVS.mask_tune || src.contours.size() != 0){ 46 | 47 | src.points = agribotVS.getContureCenters(src.image, src.contours); 48 | 49 | src.nh_points = agribotVS.filterContures(src.image, src.contours); 50 | 51 | agribotVS.is_in_neigbourhood(src); 52 | 53 | src.lines = agribotVS.FitLineOnContures(src.image, src.nh_points); 54 | }else{ 55 | cout << "Numner of contures: " << src.contours.size() << endl; 56 | publishVelocity(0); 57 | } 58 | } 59 | void AgribotVSNodeHandler::imageFrontCalllBack(const sensor_msgs::ImageConstPtr& msg) { 60 | try { 61 | agribotVS.front_cam.image = cv_bridge::toCvShare(msg, "bgr8")->image; 62 | CropRow_Tracking(agribotVS.front_cam); 63 | 64 | string str; 65 | stringstream stream,stream1,stream2; 66 | stream << agribotVS.front_cam.points.size(); 67 | stream1 << agribotVS.front_cam.nh_points.size(); 68 | stream2 << agribotVS.camera_ID; 69 | str = "Number of Points: " + stream.str() + " nh_points: " + stream1.str() + " Cam ID: " + stream2.str(); 70 | cv::putText(agribotVS.front_cam.image, str, cv::Point(40, 20), // Coordinates 71 | cv::FONT_HERSHEY_COMPLEX_SMALL, // Font 72 | 0.75, // Scale. 2.0 = 2x bigger 73 | cv::Scalar(0, 0, 255), // BGR Color 74 | 1); // Line Thickness (Optional) 75 | 76 | } catch (cv_bridge::Exception& e) { 77 | ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); 78 | } 79 | } 80 | void AgribotVSNodeHandler::imageBackCalllBack(const sensor_msgs::ImageConstPtr& msg) { 81 | try { 82 | agribotVS.back_cam.image = cv_bridge::toCvShare(msg, "bgr8")->image; 83 | CropRow_Tracking(agribotVS.back_cam); 84 | 85 | string str; 86 | stringstream stream,stream1,stream2; 87 | stream << agribotVS.back_cam.points.size(); 88 | stream1 << agribotVS.back_cam.nh_points.size(); 89 | stream2 << agribotVS.camera_ID; 90 | str = "Number of Points: " + stream.str() + " nh_points: " + stream1.str() + " Cam ID: " + stream2.str(); 91 | cv::putText(agribotVS.back_cam.image, str, cv::Point(40, 20), // Coordinates 92 | cv::FONT_HERSHEY_COMPLEX_SMALL, // Font 93 | 0.75, // Scale. 2.0 = 2x bigger 94 | cv::Scalar(0, 0, 255), // BGR Color 95 | 1); // Line Thickness (Optional) 96 | 97 | } catch (cv_bridge::Exception& e) { 98 | ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); 99 | } 100 | } 101 | void AgribotVSNodeHandler::imuCallBack(const sensor_msgs::Imu::ConstPtr& msg){ 102 | tf::Quaternion quat; 103 | tf::quaternionMsgToTF(msg->orientation, quat); 104 | tf::Matrix3x3 m(quat); 105 | m.getRPY(imu_roll, imu_pitch, imu_yaw); 106 | } 107 | void AgribotVSNodeHandler::amclPoseCallBack(const geometry_msgs::PoseStamped& msg) { 108 | tf::Quaternion q(msg.pose.orientation.x, msg.pose.orientation.y, 109 | msg.pose.orientation.z, msg.pose.orientation.w); 110 | tf::Matrix3x3 m(q); 111 | m.getRPY(mocap_roll, mocap_pitch, mocap_yaw); 112 | agribotVS.RotationVec[0] = 0; 113 | agribotVS.RotationVec[1] = 0; 114 | agribotVS.RotationVec[2] = mocap_yaw; 115 | agribotVS.TransVec[0] = msg.pose.position.x; 116 | agribotVS.TransVec[1] = msg.pose.position.y; 117 | agribotVS.TransVec[2] = msg.pose.position.z; 118 | } 119 | void AgribotVSNodeHandler::odomCallBack(const nav_msgs::Odometry::ConstPtr& msg) { 120 | agribotVS.RobotPose[0] = msg->pose.pose.position.x; 121 | agribotVS.RobotPose[1] = msg->pose.pose.position.y; 122 | agribotVS.RobotPose[2] = msg->pose.pose.position.z; 123 | 124 | std::vector Orineration = agribotVS.getEulerAngles(msg); 125 | 126 | agribotVS.RobotPose[3] = Orineration[0]; // x oreintation 127 | agribotVS.RobotPose[4] = Orineration[1]; // y oreintation 128 | agribotVS.RobotPose[5] = Orineration[2]; // z oreintation 129 | 130 | agribotVS.RobotLinearVelocities[0] = msg->twist.twist.linear.x; 131 | agribotVS.RobotLinearVelocities[1] = msg->twist.twist.linear.y; 132 | agribotVS.RobotLinearVelocities[2] = msg->twist.twist.linear.z; 133 | 134 | agribotVS.RobotAngularVelocities[0] = msg->twist.twist.angular.x; 135 | agribotVS.RobotAngularVelocities[1] = msg->twist.twist.angular.y; 136 | agribotVS.RobotAngularVelocities[2] = msg->twist.twist.angular.z; 137 | } 138 | void AgribotVSNodeHandler::StopForSec(float delay) { 139 | agribotVS.VelocityMsg.angular.z = 0.0; 140 | agribotVS.VelocityMsg.linear.x = 0.0; 141 | if(agribotVS.publish_cmd_vel)VSVelocityPub.publish(agribotVS.VelocityMsg); 142 | ros::Duration(delay).sleep(); // sleep for half a second 143 | } 144 | void AgribotVSNodeHandler::publishVelocity(int _in) { 145 | if(!agribotVS.publish_linear_vel)agribotVS.VelocityMsg.linear.x = 0.0; 146 | if(_in == 0){ 147 | agribotVS.VelocityMsg.linear.x = 0.0; 148 | agribotVS.VelocityMsg.angular.z = 0.0; 149 | if(agribotVS.publish_cmd_vel) 150 | VSVelocityPub.publish(agribotVS.VelocityMsg); 151 | }else{ 152 | if(agribotVS.publish_cmd_vel){ 153 | VSVelocityPub.publish(agribotVS.VelocityMsg); 154 | } 155 | } 156 | Log_pub.publish(agribotVS.VSMsg); 157 | } 158 | 159 | } // namespace agribot_vs 160 | --------------------------------------------------------------------------------