From 3e7672766509fb500f8af447bb482467a20e789b Mon Sep 17 00:00:00 2001 From: tongtybj Date: Sun, 23 Apr 2017 02:43:29 +0900 Subject: [PATCH 01/10] Seperate the different functions in previous "tree_detector.cpp" into 1) laser part, 2) vision part, 3) target tree tracking part --- jsk_uav_forest_common/launch/debug.launch | 19 +- jsk_uav_forest_perception/CMakeLists.txt | 20 +- ...{ColorRegionDetector.cfg => HsvFilter.cfg} | 2 +- .../color_region_detection_nodelets.xml | 7 - ...an_filters.h => laser_clustering_filter.h} | 104 +++-- .../{tree_detector.h => tree_tracking.h} | 52 +-- ...or_region_detector.h => vision_detector.h} | 78 +++- .../laser_filters_plugins.xml | 6 +- .../launch/image_process.launch | 13 +- .../launch/laser_process.launch | 9 +- .../launch/tree_detector.launch | 11 +- jsk_uav_forest_perception/package.xml | 2 +- .../param/LaserShadowFilter.yaml | 2 +- .../param/TreeFilter.yaml | 6 +- ...ionDetection.yaml => VisionDetection.yaml} | 2 +- .../src/color_region_detector.cpp | 180 --------- ...ilters.cpp => laser_clustering_filter.cpp} | 6 +- .../src/tree_detector.cpp | 359 ------------------ .../src/tree_tracking.cpp | 185 +++++++++ ...tector_node.cpp => tree_tracking_node.cpp} | 6 +- .../src/vision_detector.cpp | 285 ++++++++++++++ .../vision_detection_nodelets.xml | 7 + 22 files changed, 677 insertions(+), 684 deletions(-) rename jsk_uav_forest_perception/cfg/{ColorRegionDetector.cfg => HsvFilter.cfg} (88%) delete mode 100644 jsk_uav_forest_perception/color_region_detection_nodelets.xml rename jsk_uav_forest_perception/include/jsk_uav_forest_perception/{laser_scan_filters.h => laser_clustering_filter.h} (57%) rename jsk_uav_forest_perception/include/jsk_uav_forest_perception/{tree_detector.h => tree_tracking.h} (70%) rename jsk_uav_forest_perception/include/jsk_uav_forest_perception/{color_region_detector.h => vision_detector.h} (59%) rename jsk_uav_forest_perception/param/{ColorRegionDetection.yaml => VisionDetection.yaml} (85%) delete mode 100644 jsk_uav_forest_perception/src/color_region_detector.cpp rename jsk_uav_forest_perception/src/{laser_scan_filters.cpp => laser_clustering_filter.cpp} (87%) delete mode 100644 jsk_uav_forest_perception/src/tree_detector.cpp create mode 100644 jsk_uav_forest_perception/src/tree_tracking.cpp rename jsk_uav_forest_perception/src/{tree_detector_node.cpp => tree_tracking_node.cpp} (93%) create mode 100644 jsk_uav_forest_perception/src/vision_detector.cpp create mode 100644 jsk_uav_forest_perception/vision_detection_nodelets.xml diff --git a/jsk_uav_forest_common/launch/debug.launch b/jsk_uav_forest_common/launch/debug.launch index 4db5835..50e88bc 100644 --- a/jsk_uav_forest_common/launch/debug.launch +++ b/jsk_uav_forest_common/launch/debug.launch @@ -2,12 +2,20 @@ - + + ############## Viewer ############### ############## Odometry ############### - + + + + + + + + @@ -26,11 +34,8 @@ - - - - - + + diff --git a/jsk_uav_forest_perception/CMakeLists.txt b/jsk_uav_forest_perception/CMakeLists.txt index 026d650..2b06170 100644 --- a/jsk_uav_forest_perception/CMakeLists.txt +++ b/jsk_uav_forest_perception/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(OpenCV REQUIRED) message(WARNING "OPENCV ${OpenCV_VERSION} FOUND") generate_dynamic_reconfigure_options( - cfg/ColorRegionDetector.cfg + cfg/HsvFilter.cfg ) @@ -40,18 +40,18 @@ include_directories( ${catkin_INCLUDE_DIRS} ) +add_library(laser_clustering_filter SHARED src/laser_clustering_filter.cpp) +target_link_libraries(laser_clustering_filter ${catkin_LIBRARIES}) -add_library(color_region_detection src/color_region_detector.cpp) -target_link_libraries(color_region_detection ${catkin_LIBRARIES}) -add_dependencies(color_region_detection ${PROJECT_NAME}_gencfg) +add_library(vision_detection src/vision_detector.cpp) +target_link_libraries(vision_detection ${catkin_LIBRARIES}) +add_dependencies(vision_detection ${PROJECT_NAME}_gencfg) -add_library(jsk_laser_scan_filters SHARED src/laser_scan_filters.cpp) -target_link_libraries(jsk_laser_scan_filters ${catkin_LIBRARIES}) -add_library(tree_detector src/tree_detector.cpp) -target_link_libraries(tree_detector ${catkin_LIBRARIES}) -add_executable(tree_detector_node src/tree_detector_node.cpp) -target_link_libraries (tree_detector_node ${catkin_LIBRARIES} tree_detector) +add_library(tree_tracking src/tree_tracking.cpp) +target_link_libraries(tree_tracking ${catkin_LIBRARIES}) +add_executable(tree_tracking_node src/tree_tracking_node.cpp) +target_link_libraries (tree_tracking_node ${catkin_LIBRARIES} tree_tracking) ############# diff --git a/jsk_uav_forest_perception/cfg/ColorRegionDetector.cfg b/jsk_uav_forest_perception/cfg/HsvFilter.cfg similarity index 88% rename from jsk_uav_forest_perception/cfg/ColorRegionDetector.cfg rename to jsk_uav_forest_perception/cfg/HsvFilter.cfg index 3d90521..3fb6699 100755 --- a/jsk_uav_forest_perception/cfg/ColorRegionDetector.cfg +++ b/jsk_uav_forest_perception/cfg/HsvFilter.cfg @@ -13,4 +13,4 @@ gen.add("color_hsv_s_max", int_t, 0, "S upper bound", 255, 0, 255) gen.add("color_hsv_v_min", int_t, 0, "V lower bound", 2, 0, 255) gen.add("color_hsv_v_max", int_t, 0, "V upper bound", 216, 0, 255) -exit(gen.generate(PACKAGE, "jsk_uav_forest_perception", "ColorRegionDetector")) +exit(gen.generate(PACKAGE, "jsk_uav_forest_perception", "HsvFilter")) diff --git a/jsk_uav_forest_perception/color_region_detection_nodelets.xml b/jsk_uav_forest_perception/color_region_detection_nodelets.xml deleted file mode 100644 index d8e6481..0000000 --- a/jsk_uav_forest_perception/color_region_detection_nodelets.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Nodelet for color region detector based on hsv filter. - - - diff --git a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/laser_scan_filters.h b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/laser_clustering_filter.h similarity index 57% rename from jsk_uav_forest_perception/include/jsk_uav_forest_perception/laser_scan_filters.h rename to jsk_uav_forest_perception/include/jsk_uav_forest_perception/laser_clustering_filter.h index 13f5ca0..3eb648b 100644 --- a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/laser_scan_filters.h +++ b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/laser_clustering_filter.h @@ -38,21 +38,23 @@ #include #include +using namespace std; + namespace laser_filters{ -/** @b ScanTreeFilter is a simple filter that filters shadow points in a laser scan line +/** @b LaserClusteringFilter is a simple filter that filters shadow points in a laser scan line */ -class ScanTreeFilter : public filters::FilterBase +class LaserClusteringFilter : public filters::FilterBase { public: double max_radius_; // Filter angle threshold int min_points_; - + bool verbose_; //////////////////////////////////////////////////////////////////////////////// - ScanTreeFilter () + LaserClusteringFilter () { @@ -74,11 +76,16 @@ class ScanTreeFilter : public filters::FilterBase ROS_INFO("Error: TreeFilter was not given min_points.\n"); return false; } + if (!filters::FilterBase::getParam(std::string("verbose"), verbose_)) + { + verbose_ = false; + } + return true; } //////////////////////////////////////////////////////////////////////////////// - virtual ~ScanTreeFilter () { } + virtual ~LaserClusteringFilter () { } //////////////////////////////////////////////////////////////////////////////// /** \brief @@ -95,49 +102,70 @@ class ScanTreeFilter : public filters::FilterBase std::vector > range_blobs; std::vector range_blob; for (unsigned int i = 0; i < scan_in.ranges.size (); i++) - { - scan_out.ranges[i] = -1.0 * fabs(scan_in.ranges[i]); // set all ranges to invalid (*) - if ( scan_in.ranges[i] < 0 || std::isnan(scan_in.ranges[i])) { + { + scan_out.ranges[i] = -1.0 * fabs(scan_in.ranges[i]); // set all ranges to invalid (*) + if ( scan_in.ranges[i] < 0 || std::isnan(scan_in.ranges[i])) { if ( range_blob.size() > min_points_ ) { - range_blobs.push_back(range_blob); + range_blobs.push_back(range_blob); } range_blob.clear(); - }else{ + }else{ range_blob.push_back(i); + } } - } if ( range_blob.size() > min_points_ ) { - range_blobs.push_back(range_blob); + range_blobs.push_back(range_blob); } - // for each blob calculate center and radius + + /* for each blob calculate center and radius */ for (unsigned int i = 0; i < range_blobs.size(); i++) { - int size = range_blobs[i].size(); - // check center of blob - double center_x = 0, center_y = 0; - for (unsigned int j = 0; j < size; j++) { - double x = scan_in.ranges[range_blobs[i][j]]; - double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment; - center_x += x; - center_y += y; - } - center_x /= size; - center_y /= size; - - // check range of blob - double radius = 0; - for (unsigned int j = 0; j < size; j++) { - double x = scan_in.ranges[range_blobs[i][j]]; - double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment; - if ( radius < fabs(center_x - x) ) radius = fabs(center_x - x) ; - if ( radius < fabs(center_y - y) ) radius = fabs(center_y - y) ; + int size = range_blobs[i].size(); +#if 0 // previous radisu calculation, has some problem + /* check center of blob */ + double center_x = 0, center_y = 0; + for (unsigned int j = 0; j < size; j++) { + double x = scan_in.ranges[range_blobs[i][j]]; + double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment; + center_x += x; + center_y += y; + } + center_x /= size; + center_y /= size; + + /* check range of blob */ + double radius = 0; + for (unsigned int j = 0; j < size; j++) { + double x = scan_in.ranges[range_blobs[i][j]]; + double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment; + if ( radius < fabs(center_x - x) ) radius = fabs(center_x - x) ; + if ( radius < fabs(center_y - y) ) radius = fabs(center_y - y) ; + } +#else // very rough proximation + float angle = (size - 1) * scan_in.angle_increment; + float length = 0; + float radius = 0; + for (unsigned int j = 0; j < size - 1; j++) { + float x_curr = scan_in.ranges[range_blobs[i][j]]; + float x_next = scan_in.ranges[range_blobs[i][j + 1]]; // cos(scan_in.angle_increment) = 1 + float y_next = scan_in.ranges[range_blobs[i][j + 1]] * scan_in.angle_increment; // sin(scan_in.angle_increment) = scan_in.angle_increment + length += sqrt((x_curr - x_next) * (x_curr - x_next) + y_next * y_next); + } + radius = length / (M_PI - angle); + if(verbose_) + { + cout << i << ": direction: " << (range_blobs[i][0] + size/2) * scan_in.angle_increment + scan_in.angle_min + << "[rad]; distance: " << scan_in.ranges[range_blobs[i][0] + size/2] + << "[m]; size: " << size << "; radius: " + << radius << "[m]; length: " << length << "[m]; angle: " << angle << "[rad]" << endl; } +#endif - ROS_DEBUG_STREAM("blob center " << center_x << " " << center_y << ", radius " << radius << ", num of ponits " << size); - if ( radius < max_radius_ ) { - indices_to_publish.insert(range_blobs[i][0] + size/2); - } + if ( radius < max_radius_ ) { + indices_to_publish.insert(range_blobs[i][0] + size/2); + } } - ROS_DEBUG("ScanTreeFilter %d Points from scan with min radius: %.2f, num of pints: %d", (int)indices_to_publish.size(), max_radius_, min_points_); + + for ( std::set::iterator it = indices_to_publish.begin(); it != indices_to_publish.end(); ++it) { scan_out.ranges[*it] = fabs(scan_in.ranges[*it]); // valid only the ranges that passwd the test (*) @@ -150,4 +178,4 @@ class ScanTreeFilter : public filters::FilterBase } ; } -#endif //LASER_SCAN_TREE_FILTER_H +#endif //LASER_CUSTERING_FILTER_H diff --git a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/tree_detector.h b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/tree_tracking.h similarity index 70% rename from jsk_uav_forest_perception/include/jsk_uav_forest_perception/tree_detector.h rename to jsk_uav_forest_perception/include/jsk_uav_forest_perception/tree_tracking.h index e2dd3fd..7d7dd92 100644 --- a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/tree_detector.h +++ b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/tree_tracking.h @@ -33,17 +33,19 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef TREE_DETECTOR_H_ -#define TREE_DETECTOR_H_ +#ifndef TREE_TRACKING_H_ +#define TREE_TRACKING_H_ /* ros */ #include + +/* message filter */ #include #include /* ros msg/srv */ #include -#include +#include #include #include #include @@ -51,48 +53,37 @@ using namespace std; -class TreeDetector +class TreeTracking { public: - TreeDetector(ros::NodeHandle nh, ros::NodeHandle nhp); - ~TreeDetector(){} + TreeTracking(ros::NodeHandle nh, ros::NodeHandle nhp); + ~TreeTracking(){} + typedef message_filters::TimeSynchronizer SyncPolicy; private: ros::NodeHandle nh_, nhp_; + + boost::shared_ptr sync_; + message_filters::Subscriber sub_sync_vision_detection_; + message_filters::Subscriber sub_sync_scan_; + ros::Subscriber sub_uav_odom_; - ros::Subscriber sub_color_region_center_; - ros::Subscriber sub_camera_info_; - ros::Subscriber sub_clustered_laser_scan_; ros::Subscriber sub_laser_scan_; - ros::Subscriber sub_perception_start_; + ros::Publisher pub_tree_location_; ros::Publisher pub_tree_global_location_; - ros::Publisher pub_tree_cluster_; string uav_odom_topic_name_; - string color_region_center_topic_name_; - string camera_info_topic_name_; string laser_scan_topic_name_; + string vision_detection_topic_name_; string tree_location_topic_name_; string tree_global_location_topic_name_; string tree_cluster_topic_name_; - string perception_start_topic_name_; - bool tree_cluster_pub_; - double color_region_tree_clustering_angle_diff_thre_; - double first_detection_depth_thre_; + double target_tree_drift_thre_; double uav_tilt_thre_; bool verbose_; - bool camera_info_update_; - bool color_region_update_; - bool detector_from_image_; - - double camera_fx_, camera_fy_, camera_cx_, camera_cy_; - double color_region_direction_; - double clurstering_max_radius_; - int clurstering_min_points_; - int target_tree_index_; double target_theta_, target_dist_; tf::Vector3 uav_odom_; float uav_roll_, uav_pitch_, uav_yaw_; @@ -101,14 +92,11 @@ class TreeDetector void subscribe(); void unsubscribe(); - void treeClustering(const sensor_msgs::LaserScan& scan_in, vector& cluster_index); - void circleFitting(); - + void visionDetectionCallback(const sensor_msgs::LaserScanConstPtr& laser_msg, const geometry_msgs::Vector3StampedConstPtr& vision_detection_msg); void laserScanCallback(const sensor_msgs::LaserScanConstPtr& laser_msg); - void colorRegionCallback(const geometry_msgs::PointStampedConstPtr& color_region_center_msg); - void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info); void uavOdomCallback(const nav_msgs::OdometryConstPtr& uav_msg); - void perceptionStartCallback(const std_msgs::BoolConstPtr& msg); + + }; #endif diff --git a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/color_region_detector.h b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector.h similarity index 59% rename from jsk_uav_forest_perception/include/jsk_uav_forest_perception/color_region_detector.h rename to jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector.h index 99eea19..4a02ab9 100644 --- a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/color_region_detector.h +++ b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector.h @@ -34,21 +34,29 @@ *********************************************************************/ -#ifndef COLOR_REGION_DETECTOR_H_ -#define COLOR_REGION_DETECTOR_H_ +#ifndef VISION_DETECTOR_H_ +#define VISION_DETECTOR_H_ /* ros */ #include #include +/* message filter */ +#include +#include +#include + /* cfg */ #include -#include +#include /* ros msg */ #include #include +#include #include +#include +#include /* cv */ #include @@ -59,38 +67,72 @@ #include #include -namespace color_region_detection +using namespace std; + +namespace vision_detection { - class ColorRegionDetector: public nodelet::Nodelet + class VisionDetector: public nodelet::Nodelet { public: - ColorRegionDetector(){} - ~ColorRegionDetector(); - typedef jsk_uav_forest_perception::ColorRegionDetectorConfig Config; + VisionDetector(); + ~VisionDetector(); + typedef message_filters::sync_policies::ApproximateTime SyncPolicy; + typedef jsk_uav_forest_perception::HsvFilterConfig Config; private: ros::NodeHandle pnh_; - ros::Subscriber sub_camera_image_; + boost::shared_ptr< message_filters::Synchronizer > sync_; + message_filters::Subscriber sub_image_; + message_filters::Subscriber sub_scan_; + ros::Subscriber sub_camera_info_; + ros::Subscriber sub_detection_start_; ros::Publisher pub_target_image_; + ros::Publisher pub_target_direction_; ros::Publisher pub_target_image_center_; boost::shared_ptr > srv_; - std::string image_topic_name_; - std::string out_image_topic_name_; - std::string camera_info_topic_name_; + /* rosparam */ + string image_topic_name_; + string camera_info_topic_name_; + string laser_scan_topic_name_; + string target_image_topic_name_; + string target_direction_topic_name_; + string target_image_center_topic_name_; + string detection_start_topic_name_; - int contour_color_r_, contour_color_g_, contour_color_b_; - cv::Scalar contour_color_; + bool detect_once_; + bool verbose_; int hsv_lower_bound_[3]; int hsv_upper_bound_[3]; - bool camera_info_update_; - double camera_fx_, camera_fy_; - void configCallback(Config &new_config, uint32_t level); + int contour_color_r_, contour_color_g_, contour_color_b_; + cv::Scalar contour_color_; + double color_region_tree_clustering_angle_diff_thre_; + double first_detection_depth_thre_; + + /* base var */ + double camera_fx_, camera_fy_, camera_cx_, camera_cy_; + virtual void onInit(); - void cameraDataCallback(const sensor_msgs::ImageConstPtr& image_msg); + void configCallback(Config &new_config, uint32_t level); + + void cameraScanCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg); void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info); + void startDetectionCallback(const std_msgs::BoolConstPtr& msg); + + void subscribe() + { + sub_image_.subscribe(); //start + sub_scan_.subscribe(); //start + } + + void unsubscribe() + { + sub_image_.unsubscribe(); //stop + sub_scan_.unsubscribe(); //stop + } + }; } diff --git a/jsk_uav_forest_perception/laser_filters_plugins.xml b/jsk_uav_forest_perception/laser_filters_plugins.xml index d04c8b7..f1aca41 100644 --- a/jsk_uav_forest_perception/laser_filters_plugins.xml +++ b/jsk_uav_forest_perception/laser_filters_plugins.xml @@ -1,9 +1,9 @@ - - + - This is a filter which extract tree from a laser. + This is a filter which do clustering from a laser. diff --git a/jsk_uav_forest_perception/launch/image_process.launch b/jsk_uav_forest_perception/launch/image_process.launch index 1a856fc..548a947 100644 --- a/jsk_uav_forest_perception/launch/image_process.launch +++ b/jsk_uav_forest_perception/launch/image_process.launch @@ -5,17 +5,22 @@ - + - + - + + + + + + diff --git a/jsk_uav_forest_perception/launch/laser_process.launch b/jsk_uav_forest_perception/launch/laser_process.launch index 857bbbc..2ef07ba 100644 --- a/jsk_uav_forest_perception/launch/laser_process.launch +++ b/jsk_uav_forest_perception/launch/laser_process.launch @@ -7,12 +7,11 @@ - - - + - ---> + + diff --git a/jsk_uav_forest_perception/launch/tree_detector.launch b/jsk_uav_forest_perception/launch/tree_detector.launch index ef1f52d..7b2b9e4 100644 --- a/jsk_uav_forest_perception/launch/tree_detector.launch +++ b/jsk_uav_forest_perception/launch/tree_detector.launch @@ -3,7 +3,7 @@ - + @@ -15,15 +15,10 @@ - - - - - - + + - diff --git a/jsk_uav_forest_perception/package.xml b/jsk_uav_forest_perception/package.xml index 393c60d..95edddd 100644 --- a/jsk_uav_forest_perception/package.xml +++ b/jsk_uav_forest_perception/package.xml @@ -43,7 +43,7 @@ tf - + diff --git a/jsk_uav_forest_perception/param/LaserShadowFilter.yaml b/jsk_uav_forest_perception/param/LaserShadowFilter.yaml index 094e41e..4261abe 100644 --- a/jsk_uav_forest_perception/param/LaserShadowFilter.yaml +++ b/jsk_uav_forest_perception/param/LaserShadowFilter.yaml @@ -2,7 +2,7 @@ scan_filter_chain: - name: range type: laser_filters/LaserScanRangeFilter params: - lower_threshold: 0 + lower_threshold: 0.3 upper_threshold: 15 - name: shadows type: laser_filters/ScanShadowsFilter diff --git a/jsk_uav_forest_perception/param/TreeFilter.yaml b/jsk_uav_forest_perception/param/TreeFilter.yaml index 56240bc..a746b8a 100644 --- a/jsk_uav_forest_perception/param/TreeFilter.yaml +++ b/jsk_uav_forest_perception/param/TreeFilter.yaml @@ -1,7 +1,7 @@ scan_filter_chain: - name: tree - type: laser_filters/ScanTreeFilter + type: laser_filters/LaserClusteringFilter params: - max_radius: 0.5 + max_radius: 0.4 min_points: 3 - + verbose: false diff --git a/jsk_uav_forest_perception/param/ColorRegionDetection.yaml b/jsk_uav_forest_perception/param/VisionDetection.yaml similarity index 85% rename from jsk_uav_forest_perception/param/ColorRegionDetection.yaml rename to jsk_uav_forest_perception/param/VisionDetection.yaml index fc5a57f..3c037df 100644 --- a/jsk_uav_forest_perception/param/ColorRegionDetection.yaml +++ b/jsk_uav_forest_perception/param/VisionDetection.yaml @@ -1,5 +1,5 @@ camera: - color_region_detection: + vision_detection: h_min: 150 s_min: 80 v_min: 100 diff --git a/jsk_uav_forest_perception/src/color_region_detector.cpp b/jsk_uav_forest_perception/src/color_region_detector.cpp deleted file mode 100644 index 088aff6..0000000 --- a/jsk_uav_forest_perception/src/color_region_detector.cpp +++ /dev/null @@ -1,180 +0,0 @@ -// -*- mode: c++ -*- -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, JSK Lab - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/o2r other materials provided - * with the distribution. - * * Neither the name of the JSK Lab nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - - -#include "jsk_uav_forest_perception/color_region_detector.h" - -namespace color_region_detection -{ - ColorRegionDetector::~ColorRegionDetector() - { - - } - - void ColorRegionDetector::onInit() - { - pnh_ = this->getPrivateNodeHandle(); - - /* ros param */ - pnh_.param("image_topic_name", image_topic_name_, std::string("/camera/image_rect")); - pnh_.param("out_image_topic_name", out_image_topic_name_, std::string("/target")); - pnh_.param("camera_info_topic_name", camera_info_topic_name_, std::string("/camera_info")); - - pnh_.param("h_min", hsv_lower_bound_[0], 0); - pnh_.param("s_min", hsv_lower_bound_[1], 0); - pnh_.param("v_min", hsv_lower_bound_[2], 0); - pnh_.param("h_max", hsv_upper_bound_[0], 255); - pnh_.param("s_max", hsv_upper_bound_[1], 255); - pnh_.param("v_max", hsv_upper_bound_[2], 255); - - pnh_.param("contour_color_r", contour_color_r_, 255); - pnh_.param("contour_color_g", contour_color_g_, 255); - pnh_.param("contour_color_b", contour_color_b_, 255); - contour_color_ = cv::Scalar(contour_color_r_, contour_color_g_, contour_color_b_); - - /* ros pub and sub */ - pub_target_image_ = pnh_.advertise(out_image_topic_name_, 1); - pub_target_image_center_ = pnh_.advertise("/object_image_center", 1); - - sub_camera_image_ = pnh_.subscribe(image_topic_name_, 1, &ColorRegionDetector::cameraDataCallback, this); - sub_camera_info_ = pnh_.subscribe(camera_info_topic_name_, 1, &ColorRegionDetector::cameraInfoCallback, this); - - /* ros service */ - srv_ = boost::make_shared > (pnh_); - dynamic_reconfigure::Server::CallbackType f = - boost::bind (&ColorRegionDetector::configCallback, this, _1, _2); - srv_->setCallback (f); - } - - void ColorRegionDetector::configCallback(Config &new_config, uint32_t level) - { - if(!new_config.enable) return; - - ROS_WARN("New config"); - - hsv_lower_bound_[0] = new_config.color_hsv_h_min; - hsv_upper_bound_[0] = new_config.color_hsv_h_max; - hsv_lower_bound_[1] = new_config.color_hsv_s_min; - hsv_upper_bound_[1] = new_config.color_hsv_s_max; - hsv_lower_bound_[2] = new_config.color_hsv_v_min; - hsv_upper_bound_[2] = new_config.color_hsv_v_max; - } - - void ColorRegionDetector::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info) - { - if(camera_info_update_) return; - camera_fx_ = camera_info->K[0]; - camera_fy_ = camera_info->K[4]; - - if(camera_info->K[0] > 0) - { - camera_info_update_ = true; - } - } - - void ColorRegionDetector::cameraDataCallback(const sensor_msgs::ImageConstPtr& image_msg) - { - cv::Mat src_image = cv_bridge::toCvCopy(image_msg, image_msg->encoding)->image; - - /* hsv filter */ - cv::Mat hsv_image, hsv_image_mask; - if(image_msg->encoding == sensor_msgs::image_encodings::RGB8) - { - cv::cvtColor(src_image, hsv_image, CV_RGB2HSV); - } - else if(image_msg->encoding == sensor_msgs::image_encodings::BGR8) - { - cv::cvtColor(src_image, hsv_image, CV_BGR2HSV); - } - - if(hsv_lower_bound_[0] < hsv_upper_bound_[0]) - { - cv::inRange(hsv_image, - cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]), - cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]), - hsv_image_mask); - } - else - { - cv::Scalar lower_color_range_0 = cv::Scalar(0, hsv_lower_bound_[1], hsv_lower_bound_[2]); - cv::Scalar upper_color_range_0 = cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]); - cv::Scalar lower_color_range_360 = cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]); - cv::Scalar upper_color_range_360 = cv::Scalar(180, hsv_upper_bound_[1], hsv_upper_bound_[2]); - cv::Mat output_image_0, output_image_360; - cv::inRange(hsv_image, lower_color_range_0, upper_color_range_0, output_image_0); - cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360); - hsv_image_mask = output_image_0 | output_image_360; - } - - /* find countour */ - std::vector > contours; - std::vector hierarchy; - cv::findContours(hsv_image_mask, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cv::Point(0,0)); - if(contours.size() > 0) - { - float max_contour_area = 0; - float max_contour = 0; - for(int i = 0; i < contours.size(); ++i) - { - if (cv::contourArea(contours[i]) > max_contour_area) - { - max_contour_area = cv::contourArea(contours[i]); - max_contour = i; - } - } - cv::Moments contour_moments = cv::moments(contours[max_contour], true); - geometry_msgs::PointStamped contour_center; - contour_center.header = image_msg->header; - contour_center.point.x = contour_moments.m10 / contour_moments.m00; - contour_center.point.y = contour_moments.m01 / contour_moments.m00; - pub_target_image_center_.publish(contour_center); - /* draw */ - cv::circle(src_image, cv::Point(contour_center.point.x, contour_center.point.y), 3, cv::Scalar(255, 0, 0), 5); - cv::drawContours(src_image, contours, max_contour, contour_color_, 3); - - /* publish */ - pub_target_image_.publish(cv_bridge::CvImage(image_msg->header, - image_msg->encoding, - src_image).toImageMsg()); - } - else - pub_target_image_.publish(cv_bridge::CvImage(image_msg->header, - sensor_msgs::image_encodings::MONO8, - hsv_image_mask).toImageMsg()); - } -} - -#include -PLUGINLIB_EXPORT_CLASS (color_region_detection::ColorRegionDetector, nodelet::Nodelet); diff --git a/jsk_uav_forest_perception/src/laser_scan_filters.cpp b/jsk_uav_forest_perception/src/laser_clustering_filter.cpp similarity index 87% rename from jsk_uav_forest_perception/src/laser_scan_filters.cpp rename to jsk_uav_forest_perception/src/laser_clustering_filter.cpp index 949e77f..a77b1fc 100644 --- a/jsk_uav_forest_perception/src/laser_scan_filters.cpp +++ b/jsk_uav_forest_perception/src/laser_clustering_filter.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2009, Willow Garage, Inc. + * Copyright (c) 2017, JSK. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -27,11 +27,11 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "jsk_uav_forest_perception/laser_scan_filters.h" +#include "jsk_uav_forest_perception/laser_clustering_filter.h" #include "sensor_msgs/LaserScan.h" #include "filters/filter_base.h" #include "pluginlib/class_list_macros.h" -PLUGINLIB_DECLARE_CLASS(laser_filters, ScanTreeFilter, laser_filters::ScanTreeFilter, filters::FilterBase) +PLUGINLIB_DECLARE_CLASS(laser_filters, LaserClusteringFilter, laser_filters::LaserClusteringFilter, filters::FilterBase) diff --git a/jsk_uav_forest_perception/src/tree_detector.cpp b/jsk_uav_forest_perception/src/tree_detector.cpp deleted file mode 100644 index ed5d76f..0000000 --- a/jsk_uav_forest_perception/src/tree_detector.cpp +++ /dev/null @@ -1,359 +0,0 @@ -// -*- mode: c++ -*- -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, JSK Lab - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/o2r other materials provided - * with the distribution. - * * Neither the name of the JSK Lab nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include "jsk_uav_forest_perception/tree_detector.h" - -TreeDetector::TreeDetector(ros::NodeHandle nh, ros::NodeHandle nhp): - nh_(nh), nhp_(nhp), - camera_info_update_(false), - camera_fx_(0), camera_fy_(0), camera_cx_(0), camera_cy_(0), - color_region_update_(false), - color_region_direction_(0), - detector_from_image_(false), - target_tree_index_(0), - target_theta_(0), - target_dist_(0), - uav_odom_(0,0,0), - target_tree_global_location_(0,0,0), - uav_yaw_(0), uav_roll_(0), uav_pitch_(0) -{ - /* ros param */ - nhp_.param("uav_odom_topic_name", uav_odom_topic_name_, string("uav_odom")); - nhp_.param("color_region_center_topic_name", color_region_center_topic_name_, string("color_region_center")); - nhp_.param("camera_info_topic_name", camera_info_topic_name_, string("camera_info")); - nhp_.param("laser_scan_topic_name", laser_scan_topic_name_, string("scan")); - nhp_.param("tree_location_topic_name", tree_location_topic_name_, string("tree_location")); - nhp_.param("tree_global_location_topic_name", tree_global_location_topic_name_, string("tree_global_location")); - nhp_.param("tree_cluster_topic_name", tree_cluster_topic_name_, string("tree_cluster")); - nhp_.param("perception_start_topic_name", perception_start_topic_name_, string("perception_start")); - nhp_.param("clurstering_max_radius", clurstering_max_radius_, 0.1); - nhp_.param("clurstering_min_points", clurstering_min_points_, 3); - nhp_.param("tree_cluster_pub", tree_cluster_pub_, true); - nhp_.param("color_region_tree_clustering_angle_diff_thre", color_region_tree_clustering_angle_diff_thre_, 0.10); - nhp_.param("target_tree_drift_thre", target_tree_drift_thre_, 0.5); - nhp_.param("first_detection_depth_thre", first_detection_depth_thre_, 8.0); // [m] - nhp_.param("uav_tilt_thre", uav_tilt_thre_, 0.17); - nhp_.param("verbose", verbose_, false); - - pub_tree_location_ = nh_.advertise(tree_location_topic_name_, 1); - pub_tree_global_location_ = nh_.advertise(tree_global_location_topic_name_, 1); - pub_tree_cluster_ = nh_.advertise(tree_cluster_topic_name_, 1); - sub_perception_start_ = nh_.subscribe(perception_start_topic_name_, 1, &TreeDetector::perceptionStartCallback, this); -} - -void TreeDetector::perceptionStartCallback(const std_msgs::BoolConstPtr& msg) -{ - if(msg->data) - { - ROS_INFO("start perception"); - subscribe(); - } - else - { - ROS_INFO("stop perception"); - unsubscribe(); - } -} - -void TreeDetector::subscribe() -{ - sub_laser_scan_ = nh_.subscribe(laser_scan_topic_name_, 1, &TreeDetector::laserScanCallback, this); - sub_color_region_center_ = nh_.subscribe(color_region_center_topic_name_, 1, &TreeDetector::colorRegionCallback, this); - sub_camera_info_ = nh_.subscribe(camera_info_topic_name_, 1, &TreeDetector::cameraInfoCallback, this); - sub_uav_odom_ = nh_.subscribe(uav_odom_topic_name_, 1, &TreeDetector::uavOdomCallback, this); -} - -void TreeDetector::unsubscribe() -{ - sub_clustered_laser_scan_.shutdown(); - sub_laser_scan_.shutdown(); - sub_color_region_center_.shutdown(); - sub_camera_info_.shutdown(); - sub_uav_odom_.shutdown(); -} - -void TreeDetector::colorRegionCallback(const geometry_msgs::PointStampedConstPtr& color_region_center_msg) -{ - if(!camera_info_update_ || color_region_update_) return; - - /* TODO: we need a more robust color region selection method, i.e. histogram with a certaion motion */ - /* we calculate the angle between the x axis(forward) and the line to the color region */ - color_region_direction_ = atan2(-color_region_center_msg->point.x + camera_cx_, camera_fx_); - color_region_update_ = true; - - if(verbose_) - { - cout << "camera_cx" << camera_cx_ << endl; - cout << "color_region_center_msg->point.x" << color_region_center_msg->point.x << endl; - cout << "color regio direction" << color_region_direction_ << endl; - } -} - -void TreeDetector::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info) -{ - camera_info_update_ = true; - camera_fx_ = camera_info->K[0]; - camera_fy_ = camera_info->K[4]; - camera_cx_ = camera_info->K[2]; - camera_cy_ = camera_info->K[5]; -} - -void TreeDetector::uavOdomCallback(const nav_msgs::OdometryConstPtr& uav_msg) -{ - tf::Quaternion uav_q(uav_msg->pose.pose.orientation.x, - uav_msg->pose.pose.orientation.y, - uav_msg->pose.pose.orientation.z, - uav_msg->pose.pose.orientation.w); - tf::Matrix3x3 uav_orientation_(uav_q); - tfScalar r,p,y; - uav_orientation_.getRPY(r, p, y); - uav_odom_.setX(uav_msg->pose.pose.position.x); - uav_odom_.setY(uav_msg->pose.pose.position.y); - uav_odom_.setZ(uav_msg->pose.pose.position.z); - uav_roll_ = r; uav_pitch_ = p; uav_yaw_ = y; -} - -void TreeDetector::laserScanCallback(const sensor_msgs::LaserScanConstPtr& laser_msg) -{ - /* 1. Do the tree clustering based on the simple radius checking algorithm */ - /* 2-a. Find the most closed tree to the color region, record the target tree index */ - /* 2-b. Find the most closed tree to previos target tree index, update the target tree index */ - /* TODO: we need a more tree detection method, i.e. particle filter */ - /* tree clustering */ - vector cluster_index; - treeClustering(*laser_msg, cluster_index); - - if(!color_region_update_) return; - - if(verbose_) - ROS_INFO("receive new laser scan"); - - /* find the tree most close to the color region */ - float min_diff = 1e6; - int target_tree_index = target_tree_index_; - tf::Vector3 target_tree_global_location; - - for ( vector::iterator it = cluster_index.begin(); it != cluster_index.end(); ++it) - { - float diff = 0; - tf::Vector3 tree_global_location; - - if(!detector_from_image_) - {/* we only use the result of color_region_direction at begin, and focus within the fov of the camera image */ - float laser_direction = *it * laser_msg->angle_increment + laser_msg->angle_min; - float fov = atan2(camera_cx_, camera_fx_); //proximate the width of image with camera_cx_ - if(fabs(laser_direction) > fov) - { - if(verbose_) cout << "eliminate: laser_direction:" << laser_direction << "; " << "fov: " << fov << endl; - continue; - } - - if(laser_msg->ranges[*it] > first_detection_depth_thre_) - { - if(verbose_) cout << "eliminate: depth: " << laser_msg->ranges[*it] << endl; - continue; - } - - diff = fabs(laser_direction - color_region_direction_); - } - else - {/* calculate the distance */ - tf::Matrix3x3 rotation; - rotation.setRPY(0, 0, *it * laser_msg->angle_increment + laser_msg->angle_min + uav_yaw_); - tree_global_location = uav_odom_ + rotation * tf::Vector3(laser_msg->ranges[*it], 0, 0); - diff = (target_tree_global_location_ - tree_global_location).length(); - } - - if(verbose_) - { - cout << "tree index: " << *it << endl; - cout << "tree angle: " << *it * laser_msg->angle_increment + laser_msg->angle_min << endl; - cout << "diff: " << diff << endl; - } - - if(diff < min_diff) - { - min_diff = diff; - target_tree_index = *it; - if(detector_from_image_) target_tree_global_location = tree_global_location; - } - } - - /* update */ - if(!detector_from_image_) - { - if(min_diff < color_region_tree_clustering_angle_diff_thre_) - { - ROS_WARN("tree detector: find the target tree, angle_diff: %f", min_diff); - detector_from_image_ = true; - tf::Matrix3x3 rotation; - rotation.setRPY(0, 0, target_tree_index * laser_msg->angle_increment + laser_msg->angle_min + uav_yaw_); - target_tree_global_location_ = uav_odom_ + rotation * tf::Vector3(laser_msg->ranges[target_tree_index], 0, 0); - } - else - { - ROS_INFO_THROTTLE(1,"can not find the target tree, diff: %f", min_diff); - } - } - else - { - /* outlier check method */ - /* we only update the tree location, if the metric drift compared with previous location is within certain threshold, that is target_tree_drift_thre_ and uav_tilt_thre_. This can avoid, to some degree, the dramatic laser scan change due to the rapid attitude tilting of uav, especially the forward and backward movement by ptich tilting, along with the case that target tree hides behind other objects. We also believe that the laser scan can recover after being back to level, and the new target(tree) location has a small offset with the previous valid target location */ - /* TODO: we need a more robust outlier check method, i.e. the dynamic threshold based on the tilt angle and distance to the target */ - - if(fabs(uav_pitch_) < uav_tilt_thre_) - { - float drift = (target_tree_global_location_ - target_tree_global_location).length(); - if(verbose_) cout << "drift: " << drift << endl; - if(drift < target_tree_drift_thre_) - { - target_tree_global_location_ = target_tree_global_location; - } - else - { - ROS_WARN("lost the target tree, drift: %f, the nearest target location: [%f, %f], prev target location: [%f, %f]", drift, target_tree_global_location.x(), target_tree_global_location.y(), target_tree_global_location_.x(), target_tree_global_location_.y()); - } - } - else - { - ROS_WARN("Too much tilt: %f", uav_pitch_); - } - } - - if(verbose_) - ROS_INFO("target_tree_global_location_: [%f, %f]", target_tree_global_location_.x(), target_tree_global_location_.y()); - - /* publish the location of the target tree */ - geometry_msgs::PointStamped target_msg; - target_msg.header = laser_msg->header; - tf::Matrix3x3 rotation; - rotation.setRPY(0, 0, -uav_yaw_); - tf::Vector3 target_tree_local_location = rotation * (target_tree_global_location_ - uav_odom_); - target_msg.point.x = target_tree_local_location.x(); - target_msg.point.y = target_tree_local_location.y(); - pub_tree_location_.publish(target_msg); - geometry_msgs::PointStamped target_global_msg; - target_global_msg.header = laser_msg->header; - target_global_msg.point.x = target_tree_global_location_.x(); - target_global_msg.point.y = target_tree_global_location_.y(); - pub_tree_global_location_.publish(target_global_msg); - - - /* TODO: we need record method for multiple trees using the odom of UAV */ -} - - -void TreeDetector::treeClustering(const sensor_msgs::LaserScan& scan_in, vector& cluster_index) -{ - /* copy across all data first */ - sensor_msgs::LaserScan scan_out = scan_in; - cluster_index.resize(0); - - std::set indices_to_publish; - /* assume that all points is pass thorugh shadow filter, so each blob is separeted by invalide scan data */ - std::vector > range_blobs; - std::vector range_blob; - for (unsigned int i = 0; i < scan_in.ranges.size (); i++) - { - scan_out.ranges[i] = -1.0 * fabs(scan_in.ranges[i]); // set all ranges to invalid (*) - if ( scan_in.ranges[i] < 0 || std::isnan(scan_in.ranges[i])) { - if ( range_blob.size() > clurstering_min_points_ ) { - range_blobs.push_back(range_blob); - } - range_blob.clear(); - }else{ - range_blob.push_back(i); - } - } - if ( range_blob.size() > clurstering_min_points_ ) { - range_blobs.push_back(range_blob); - } - - /* for each blob calculate center and radius */ - for (unsigned int i = 0; i < range_blobs.size(); i++) { - int size = range_blobs[i].size(); -#if 0 // previous radisu calculation, has some problem - /* check center of blob */ - double center_x = 0, center_y = 0; - for (unsigned int j = 0; j < size; j++) { - double x = scan_in.ranges[range_blobs[i][j]]; - double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment; - center_x += x; - center_y += y; - } - center_x /= size; - center_y /= size; - - /* check range of blob */ - double radius = 0; - for (unsigned int j = 0; j < size; j++) { - double x = scan_in.ranges[range_blobs[i][j]]; - double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment; - if ( radius < fabs(center_x - x) ) radius = fabs(center_x - x) ; - if ( radius < fabs(center_y - y) ) radius = fabs(center_y - y) ; - } -#else // very rough proximation - float angle = (size - 1) * scan_in.angle_increment; - float length = 0; - float radius = 0; - for (unsigned int j = 0; j < size - 1; j++) { - float x_curr = scan_in.ranges[range_blobs[i][j]]; - float x_next = scan_in.ranges[range_blobs[i][j + 1]]; // cos(scan_in.angle_increment) = 1 - float y_next = scan_in.ranges[range_blobs[i][j + 1]] * scan_in.angle_increment; // sin(scan_in.angle_increment) = scan_in.angle_increment - length += sqrt((x_curr - x_next) * (x_curr - x_next) + y_next * y_next); - } - radius = length / (M_PI - angle); - if(verbose_) - { - cout << i << ": direction: " << (range_blobs[i][0] + size/2) * scan_in.angle_increment + scan_in.angle_min - << "[rad]; distance: " << scan_in.ranges[range_blobs[i][0] + size/2] - << "[m]; size: " << size << "; radius: " - << radius << "[m]; length: " << length << "[m]; angle: " << angle << "[rad]" << endl; - } -#endif - - if ( radius < clurstering_max_radius_ ) { - indices_to_publish.insert(range_blobs[i][0] + size/2); - } - } - - for ( std::set::iterator it = indices_to_publish.begin(); it != indices_to_publish.end(); ++it) - { - scan_out.ranges[*it] = fabs(scan_in.ranges[*it]); // valid only the ranges that passwd the test (*) - cluster_index.push_back(*it); - } - - if(tree_cluster_pub_) pub_tree_cluster_.publish(scan_out); -} diff --git a/jsk_uav_forest_perception/src/tree_tracking.cpp b/jsk_uav_forest_perception/src/tree_tracking.cpp new file mode 100644 index 0000000..e486724 --- /dev/null +++ b/jsk_uav_forest_perception/src/tree_tracking.cpp @@ -0,0 +1,185 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "jsk_uav_forest_perception/tree_tracking.h" + +TreeTracking::TreeTracking(ros::NodeHandle nh, ros::NodeHandle nhp): + nh_(nh), nhp_(nhp), + target_theta_(0), + target_dist_(0), + uav_odom_(0,0,0), + target_tree_global_location_(0,0,0), + uav_yaw_(0), uav_roll_(0), uav_pitch_(0) +{ + /* ros param */ + nhp_.param("uav_odom_topic_name", uav_odom_topic_name_, string("uav_odom")); + nhp_.param("vision_detection_topic_name", vision_detection_topic_name_, string("/object_direction")); + nhp_.param("laser_scan_topic_name", laser_scan_topic_name_, string("scan")); + nhp_.param("tree_location_topic_name", tree_location_topic_name_, string("tree_location")); + nhp_.param("tree_global_location_topic_name", tree_global_location_topic_name_, string("tree_global_location")); + + nhp_.param("target_tree_drift_thre", target_tree_drift_thre_, 0.5); + nhp_.param("uav_tilt_thre", uav_tilt_thre_, 0.17); + nhp_.param("verbose", verbose_, false); + + sub_sync_scan_.subscribe(nhp_, laser_scan_topic_name_, 1); + sub_sync_vision_detection_.subscribe(nhp_, vision_detection_topic_name_, 1); + + sync_ = boost::shared_ptr(new SyncPolicy(sub_sync_scan_, sub_sync_vision_detection_, 10)); + sync_->registerCallback(&TreeTracking::visionDetectionCallback, this); + + sub_uav_odom_ = nh_.subscribe(uav_odom_topic_name_, 1, &TreeTracking::uavOdomCallback, this); + + pub_tree_location_ = nh_.advertise(tree_location_topic_name_, 1); + pub_tree_global_location_ = nh_.advertise(tree_global_location_topic_name_, 1); +} + + +void TreeTracking::visionDetectionCallback(const sensor_msgs::LaserScanConstPtr& laser_msg, const geometry_msgs::Vector3StampedConstPtr& vision_detection_msg) +{ + ROS_INFO("receive vision detection result, time diff: %f", laser_msg->header.stamp.toSec() - vision_detection_msg->header.stamp.toSec()); + + //ROS_WARN("tree tracking: start tracking, angle_diff: %f, vision index: %d, laser index: %d, vision dist: %f, laser dist: %f", min_diff, (int)vision_detection_msg->vector.x, target_tree_index, vision_detection_msg->vector.z, laser_msg->ranges[target_tree_index]); + tf::Matrix3x3 rotation; + rotation.setRPY(0, 0, vision_detection_msg->vector.y + uav_yaw_); + target_tree_global_location_ = uav_odom_ + rotation * tf::Vector3(vision_detection_msg->vector.z, 0, 0); + + /* stop the synchronized subscribe and start laesr-only subscribe */ + sub_laser_scan_ = nh_.subscribe(laser_scan_topic_name_, 1, &TreeTracking::laserScanCallback, this); + + sub_sync_vision_detection_.unsubscribe(); //stop + sub_sync_scan_.unsubscribe(); //stop +} + + +void TreeTracking::uavOdomCallback(const nav_msgs::OdometryConstPtr& uav_msg) +{ + tf::Quaternion uav_q(uav_msg->pose.pose.orientation.x, + uav_msg->pose.pose.orientation.y, + uav_msg->pose.pose.orientation.z, + uav_msg->pose.pose.orientation.w); + tf::Matrix3x3 uav_orientation_(uav_q); + tfScalar r,p,y; + uav_orientation_.getRPY(r, p, y); + uav_odom_.setX(uav_msg->pose.pose.position.x); + uav_odom_.setY(uav_msg->pose.pose.position.y); + uav_odom_.setZ(uav_msg->pose.pose.position.z); + uav_roll_ = r; uav_pitch_ = p; uav_yaw_ = y; +} + +/* 1. Do the tree clustering based on the simple radius checking algorithm */ +/* 2. Find the most closed tree to previos target tree index, update the target tree index */ +void TreeTracking::laserScanCallback(const sensor_msgs::LaserScanConstPtr& laser_msg) +{ + if(verbose_) ROS_INFO("receive new laser scan"); + + /* tree clustering */ + vector cluster_index; + /* extract the cluster */ + for (size_t i = 0; i < laser_msg->ranges.size(); i++) + if(laser_msg->ranges[i] > 0) cluster_index.push_back(i); + + /* find the tree most close to the previous target tree */ + float min_diff = 1e6; + int target_tree_index = 0; + tf::Vector3 target_tree_global_location; + + for ( vector::iterator it = cluster_index.begin(); it != cluster_index.end(); ++it) + { + float diff = 0; + tf::Vector3 tree_global_location; + + /* calculate the distance */ + tf::Matrix3x3 rotation; + rotation.setRPY(0, 0, *it * laser_msg->angle_increment + laser_msg->angle_min + uav_yaw_); + tree_global_location = uav_odom_ + rotation * tf::Vector3(laser_msg->ranges[*it], 0, 0); + diff = (target_tree_global_location_ - tree_global_location).length(); + + if(verbose_) + { + cout << "tree index: " << *it << endl; + cout << "tree angle: " << *it * laser_msg->angle_increment + laser_msg->angle_min << endl; + cout << "diff: " << diff << endl; + } + + if(diff < min_diff) + { + min_diff = diff; + target_tree_index = *it; + target_tree_global_location = tree_global_location; + } + } + + /* update */ + if(fabs(uav_pitch_) < uav_tilt_thre_) + { + float drift = (target_tree_global_location_ - target_tree_global_location).length(); + if(verbose_) cout << "drift: " << drift << endl; + if(drift < target_tree_drift_thre_) + { + target_tree_global_location_ = target_tree_global_location; + } + else + { + ROS_WARN("lost the target tree, drift: %f, the nearest target location: [%f, %f], prev target location: [%f, %f]", drift, target_tree_global_location.x(), target_tree_global_location.y(), target_tree_global_location_.x(), target_tree_global_location_.y()); + } + } + else + { + ROS_WARN("Too much tilt: %f", uav_pitch_); + } + + + if(verbose_) + ROS_INFO("target_tree_global_location_: [%f, %f]", target_tree_global_location_.x(), target_tree_global_location_.y()); + + /* publish the location of the target tree */ + geometry_msgs::PointStamped target_msg; + target_msg.header = laser_msg->header; + tf::Matrix3x3 rotation; + rotation.setRPY(0, 0, -uav_yaw_); + tf::Vector3 target_tree_local_location = rotation * (target_tree_global_location_ - uav_odom_); + target_msg.point.x = target_tree_local_location.x(); + target_msg.point.y = target_tree_local_location.y(); + pub_tree_location_.publish(target_msg); + geometry_msgs::PointStamped target_global_msg; + target_global_msg.header = laser_msg->header; + target_global_msg.point.x = target_tree_global_location_.x(); + target_global_msg.point.y = target_tree_global_location_.y(); + pub_tree_global_location_.publish(target_global_msg); + + /* TODO: we need record method for multiple trees using the odom of UAV */ +} + diff --git a/jsk_uav_forest_perception/src/tree_detector_node.cpp b/jsk_uav_forest_perception/src/tree_tracking_node.cpp similarity index 93% rename from jsk_uav_forest_perception/src/tree_detector_node.cpp rename to jsk_uav_forest_perception/src/tree_tracking_node.cpp index fc8bdc5..43bee4e 100644 --- a/jsk_uav_forest_perception/src/tree_detector_node.cpp +++ b/jsk_uav_forest_perception/src/tree_tracking_node.cpp @@ -33,16 +33,16 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include "jsk_uav_forest_perception/tree_detector.h" +#include "jsk_uav_forest_perception/tree_tracking.h" int main (int argc, char **argv) { ros::init (argc, argv, "tree_detector"); ros::NodeHandle nh; ros::NodeHandle nh_private("~"); - TreeDetector* treeDetectorNode = new TreeDetector(nh, nh_private); + TreeTracking* treeTrackingNode = new TreeTracking(nh, nh_private); ros::spin (); - delete treeDetectorNode; + delete treeTrackingNode; return 0; } diff --git a/jsk_uav_forest_perception/src/vision_detector.cpp b/jsk_uav_forest_perception/src/vision_detector.cpp new file mode 100644 index 0000000..1f4a2a9 --- /dev/null +++ b/jsk_uav_forest_perception/src/vision_detector.cpp @@ -0,0 +1,285 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + + +#include "jsk_uav_forest_perception/vision_detector.h" + +namespace vision_detection +{ + VisionDetector::VisionDetector(): + camera_fx_(-1), camera_fy_(-1), camera_cx_(-1), camera_cy_(-1) + { + } + + VisionDetector::~VisionDetector() + { + } + + void VisionDetector::onInit() + { + pnh_ = this->getPrivateNodeHandle(); + + /* ros param */ + pnh_.param("image_topic_name", image_topic_name_, std::string("/camera/image_rect")); + pnh_.param("camera_info_topic_name", camera_info_topic_name_, std::string("camera_info")); + pnh_.param("laser_scan_topic_name", laser_scan_topic_name_, std::string("/scan")); + pnh_.param("target_image_topic_name", target_image_topic_name_, std::string("/target")); + pnh_.param("detection_start_topic_name", detection_start_topic_name_, string("/detection_start")); + pnh_.param("target_image_center_topic_name", target_image_center_topic_name_, std::string("/object_image_center")); + pnh_.param("target_direction_topic_name", target_direction_topic_name_, std::string("/object_direction")); + + pnh_.param("detect_once", detect_once_, true); + pnh_.param("verbose", verbose_, true); + + pnh_.param("color_region_tree_clustering_angle_diff_thre", color_region_tree_clustering_angle_diff_thre_, 0.10); + pnh_.param("first_detection_depth_thre", first_detection_depth_thre_, 9.0); // [m] + + pnh_.param("h_min", hsv_lower_bound_[0], 0); + pnh_.param("s_min", hsv_lower_bound_[1], 0); + pnh_.param("v_min", hsv_lower_bound_[2], 0); + pnh_.param("h_max", hsv_upper_bound_[0], 255); + pnh_.param("s_max", hsv_upper_bound_[1], 255); + pnh_.param("v_max", hsv_upper_bound_[2], 255); + + pnh_.param("contour_color_r", contour_color_r_, 255); + pnh_.param("contour_color_g", contour_color_g_, 255); + pnh_.param("contour_color_b", contour_color_b_, 255); + contour_color_ = cv::Scalar(contour_color_r_, contour_color_g_, contour_color_b_); + + /* ros pub and sub */ + sub_image_.subscribe(pnh_, image_topic_name_, 1); + sub_scan_.subscribe(pnh_, laser_scan_topic_name_, 1); + unsubscribe(); + sync_ = boost::shared_ptr >(new message_filters::Synchronizer(SyncPolicy(10))); + sync_->connectInput(sub_image_, sub_scan_); + sync_->registerCallback(&VisionDetector::cameraScanCallback, this); + + sub_detection_start_ = pnh_.subscribe(detection_start_topic_name_, 1, &VisionDetector::startDetectionCallback, this); + sub_camera_info_ = pnh_.subscribe(camera_info_topic_name_, 1, &VisionDetector::cameraInfoCallback, this); + + pub_target_image_ = pnh_.advertise(target_image_topic_name_, 1); + pub_target_direction_ = pnh_.advertise("/object_direction", 1); + pub_target_image_center_ = pnh_.advertise("/object_image_center", 1); + + /* ros service */ + srv_ = boost::make_shared > (pnh_); + dynamic_reconfigure::Server::CallbackType f = + boost::bind (&VisionDetector::configCallback, this, _1, _2); + srv_->setCallback (f); + } + + void VisionDetector::configCallback(Config &new_config, uint32_t level) + { + if(!new_config.enable) return; + + ROS_WARN("New config"); + + hsv_lower_bound_[0] = new_config.color_hsv_h_min; + hsv_upper_bound_[0] = new_config.color_hsv_h_max; + hsv_lower_bound_[1] = new_config.color_hsv_s_min; + hsv_upper_bound_[1] = new_config.color_hsv_s_max; + hsv_lower_bound_[2] = new_config.color_hsv_v_min; + hsv_upper_bound_[2] = new_config.color_hsv_v_max; + } + + void VisionDetector::startDetectionCallback(const std_msgs::BoolConstPtr& msg) + { + if(msg->data) + { + ROS_INFO("start detection"); + subscribe(); + } + else + { + ROS_INFO("stop perception"); + unsubscribe(); + } + } + + void VisionDetector::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info) + { + camera_fx_ = camera_info->K[0]; + camera_fy_ = camera_info->K[4]; + camera_cx_ = camera_info->K[2]; + camera_cy_ = camera_info->K[5]; + } + + void VisionDetector::cameraScanCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg) + { + /* debug */ + ROS_WARN("camera scan vs laser scan time diff: %f", (image_msg->header.stamp.toSec() - scan_msg->header.stamp.toSec())); + + /* laser part */ + vector cluster_index; + /* extract the cluster */ + for (size_t i = 0; i < scan_msg->ranges.size(); i++) + if(scan_msg->ranges[i] > 0) cluster_index.push_back(i); + + /* vision part */ + /* check whether the camera info is updated */ + if(camera_fx_ < -1) return; + + cv::Mat src_image = cv_bridge::toCvCopy(image_msg, image_msg->encoding)->image; + + /* independent hsv filter */ + cv::Mat hsv_image, hsv_image_mask; + if(image_msg->encoding == sensor_msgs::image_encodings::RGB8) + cv::cvtColor(src_image, hsv_image, CV_RGB2HSV); + else if(image_msg->encoding == sensor_msgs::image_encodings::BGR8) + cv::cvtColor(src_image, hsv_image, CV_BGR2HSV); + + if(hsv_lower_bound_[0] < hsv_upper_bound_[0]) + { + cv::inRange(hsv_image, + cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]), + cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]), + hsv_image_mask); + } + else + { + cv::Scalar lower_color_range_0 = cv::Scalar(0, hsv_lower_bound_[1], hsv_lower_bound_[2]); + cv::Scalar upper_color_range_0 = cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]); + cv::Scalar lower_color_range_360 = cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]); + cv::Scalar upper_color_range_360 = cv::Scalar(180, hsv_upper_bound_[1], hsv_upper_bound_[2]); + cv::Mat output_image_0, output_image_360; + cv::inRange(hsv_image, lower_color_range_0, upper_color_range_0, output_image_0); + cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360); + hsv_image_mask = output_image_0 | output_image_360; + } + + /* find countour */ + std::vector > contours; + std::vector hierarchy; + cv::findContours(hsv_image_mask, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cv::Point(0,0)); + + if(contours.size() == 0) return; + + /* find the biggest contour */ + float max_contour_area = 0; + float max_contour = 0; + for(int i = 0; i < contours.size(); ++i) + { + if (cv::contourArea(contours[i]) > max_contour_area) + { + max_contour_area = cv::contourArea(contours[i]); + max_contour = i; + } + } + cv::Moments contour_moments = cv::moments(contours[max_contour], true); + + /* publish the base information */ + geometry_msgs::PointStamped contour_center; + contour_center.header = image_msg->header; + contour_center.point.x = contour_moments.m10 / contour_moments.m00; + contour_center.point.y = contour_moments.m01 / contour_moments.m00; + pub_target_image_center_.publish(contour_center); + /* draw */ + cv::circle(src_image, cv::Point(contour_center.point.x, contour_center.point.y), 3, cv::Scalar(255, 0, 0), 5); + cv::drawContours(src_image, contours, max_contour, contour_color_, 3); + + /* publish */ + pub_target_image_.publish(cv_bridge::CvImage(image_msg->header, + image_msg->encoding, + src_image).toImageMsg()); + + + /* check whether the contour is satisfied the condistion */ + + /* we calculate the angle between the x axis(forward) and the line to the color region */ + float color_region_direction = atan2(-contour_center.point.x + camera_cx_, camera_fx_); + if(verbose_) cout << "vision detection direction" << color_region_direction << endl; + + /* Find the most closed tree to the color region, record the target tree index */ + float fov = atan2(camera_cx_, camera_fx_); //proximate the width of image with camera_cx_ + float min_diff = 1e6; + int target_tree_index = 0; + for ( vector::iterator it = cluster_index.begin(); it != cluster_index.end(); ++it) + { + float diff = 0; + + float laser_direction = *it * scan_msg->angle_increment + scan_msg->angle_min; + if(fabs(laser_direction) > fov) + { + if(verbose_) cout << "eliminate: laser_direction:" << laser_direction << "; " << "fov: " << fov << endl; + continue; + } + + if(scan_msg->ranges[*it] > first_detection_depth_thre_) + { + if(verbose_) cout << "eliminate: depth: " << scan_msg->ranges[*it] << endl; + continue; + } + + diff = fabs(laser_direction - color_region_direction); + + if(verbose_) + { + cout << "tree index: " << *it << endl; + cout << "tree angle: " << *it * scan_msg->angle_increment + scan_msg->angle_min << endl; + cout << "diff: " << diff << endl; + } + + if(diff < min_diff) + { + min_diff = diff; + target_tree_index = *it; + } + } + + /* Check whether the tree and color region is closed enough */ + if(min_diff > color_region_tree_clustering_angle_diff_thre_) + { + ROS_INFO_THROTTLE(1,"can not find the target tree, diff: %f", min_diff); + return; + } + + + /* find the target tree */ + ROS_WARN("vision detectoion: find the target tree, angle_diff: %f", min_diff); + + geometry_msgs::Vector3Stamped object_direction; + object_direction.header = scan_msg->header; + object_direction.vector.x = target_tree_index; //index + object_direction.vector.y = target_tree_index * scan_msg->angle_increment + scan_msg->angle_min; //direction + object_direction.vector.z = scan_msg->ranges[target_tree_index]; //distance + pub_target_direction_.publish(object_direction); + + /* stop the subscribe if necessary */ + if(detect_once_) unsubscribe(); + } +} + +#include +PLUGINLIB_EXPORT_CLASS (vision_detection::VisionDetector, nodelet::Nodelet); diff --git a/jsk_uav_forest_perception/vision_detection_nodelets.xml b/jsk_uav_forest_perception/vision_detection_nodelets.xml new file mode 100644 index 0000000..b51646a --- /dev/null +++ b/jsk_uav_forest_perception/vision_detection_nodelets.xml @@ -0,0 +1,7 @@ + + + + Nodelet for vision detection. + + + From 073c8a52a2173315bf35b28326db38924984739b Mon Sep 17 00:00:00 2001 From: tongtybj Date: Sun, 23 Apr 2017 05:23:29 +0900 Subject: [PATCH 02/10] Change the detection start topic name --- jsk_uav_forest_motion/script/circle_motion.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_uav_forest_motion/script/circle_motion.py b/jsk_uav_forest_motion/script/circle_motion.py index 4567f44..a756692 100755 --- a/jsk_uav_forest_motion/script/circle_motion.py +++ b/jsk_uav_forest_motion/script/circle_motion.py @@ -71,7 +71,7 @@ def init(self): self.target_pos_pub_topic_name_ = rospy.get_param("~target_pos_pub_topic_name", "uav_target_pos") self.uav_odom_sub_topic_name_ = rospy.get_param("~uav_odom_sub_topic_name", "ground_truth/state") self.tree_location_sub_topic_name_ = rospy.get_param("~tree_location_sub_topic_name", "tree_location") - self.tree_detection_start_pub_topic_name_ = rospy.get_param("~tree_detection_start_pub_topic_name", "perception_start") + self.tree_detection_start_pub_topic_name_ = rospy.get_param("~tree_detection_start_pub_topic_name", "detection_start") self.task_start_service_name_ = rospy.get_param("~task_start_service_name", "task_start") self.control_rate_ = rospy.get_param("~control_rate", 20) From b9522452e05507e0eb017591ea587dc510523878 Mon Sep 17 00:00:00 2001 From: tongtybj Date: Sun, 23 Apr 2017 05:24:07 +0900 Subject: [PATCH 03/10] Change the vision detection part to plugin based process, which is more convenient to change among different algorithm of vision filters --- jsk_uav_forest_common/launch/debug.launch | 1 - jsk_uav_forest_perception/CMakeLists.txt | 9 +- .../vision_detector.h | 139 --------- .../vision_detector_base_plugin.h | 223 ++++++++++++++ .../launch/image_process.launch | 32 +- .../launch/laser_process.launch | 2 +- .../launch/tree_detector.launch | 3 +- jsk_uav_forest_perception/package.xml | 2 +- .../param/IndependentHsvFilter.yaml | 12 + .../{TreeFilter.yaml => LaserClustering.yaml} | 0 .../param/VisionDetection.yaml | 8 - .../src/vision_detection.cpp | 83 +++++ .../src/vision_detector.cpp | 285 ------------------ .../independent_hsv_filter.cpp | 228 ++++++++++++++ .../vision_detection_nodelets.xml | 7 - .../vision_detection_plugins.xml | 7 + 16 files changed, 574 insertions(+), 467 deletions(-) delete mode 100644 jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector.h create mode 100644 jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h create mode 100644 jsk_uav_forest_perception/param/IndependentHsvFilter.yaml rename jsk_uav_forest_perception/param/{TreeFilter.yaml => LaserClustering.yaml} (100%) delete mode 100644 jsk_uav_forest_perception/param/VisionDetection.yaml create mode 100644 jsk_uav_forest_perception/src/vision_detection.cpp delete mode 100644 jsk_uav_forest_perception/src/vision_detector.cpp create mode 100644 jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp delete mode 100644 jsk_uav_forest_perception/vision_detection_nodelets.xml create mode 100644 jsk_uav_forest_perception/vision_detection_plugins.xml diff --git a/jsk_uav_forest_common/launch/debug.launch b/jsk_uav_forest_common/launch/debug.launch index 50e88bc..ed56778 100644 --- a/jsk_uav_forest_common/launch/debug.launch +++ b/jsk_uav_forest_common/launch/debug.launch @@ -25,7 +25,6 @@ ######### Perception Node ######### - diff --git a/jsk_uav_forest_perception/CMakeLists.txt b/jsk_uav_forest_perception/CMakeLists.txt index 2b06170..004c063 100644 --- a/jsk_uav_forest_perception/CMakeLists.txt +++ b/jsk_uav_forest_perception/CMakeLists.txt @@ -43,10 +43,13 @@ include_directories( add_library(laser_clustering_filter SHARED src/laser_clustering_filter.cpp) target_link_libraries(laser_clustering_filter ${catkin_LIBRARIES}) -add_library(vision_detection src/vision_detector.cpp) -target_link_libraries(vision_detection ${catkin_LIBRARIES}) -add_dependencies(vision_detection ${PROJECT_NAME}_gencfg) +add_library(vision_detection_pluginlib + src/vision_detector/independent_hsv_filter.cpp) +target_link_libraries(vision_detection_pluginlib ${catkin_LIBRARIES}) +add_dependencies(vision_detection_pluginlib ${PROJECT_NAME}_gencfg) +add_executable(vision_detection_node src/vision_detection.cpp) +target_link_libraries (vision_detection_node ${catkin_LIBRARIES}) add_library(tree_tracking src/tree_tracking.cpp) target_link_libraries(tree_tracking ${catkin_LIBRARIES}) diff --git a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector.h b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector.h deleted file mode 100644 index 4a02ab9..0000000 --- a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector.h +++ /dev/null @@ -1,139 +0,0 @@ -// -*- mode: c++ -*- -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, JSK Lab - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/o2r other materials provided - * with the distribution. - * * Neither the name of the JSK Lab nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - - -#ifndef VISION_DETECTOR_H_ -#define VISION_DETECTOR_H_ - -/* ros */ -#include -#include - -/* message filter */ -#include -#include -#include - -/* cfg */ -#include -#include - -/* ros msg */ -#include -#include -#include -#include -#include -#include - -/* cv */ -#include -#include -#include - -/* standard */ -#include -#include - -using namespace std; - -namespace vision_detection -{ - class VisionDetector: public nodelet::Nodelet - { - public: - VisionDetector(); - ~VisionDetector(); - typedef message_filters::sync_policies::ApproximateTime SyncPolicy; - typedef jsk_uav_forest_perception::HsvFilterConfig Config; - - private: - ros::NodeHandle pnh_; - boost::shared_ptr< message_filters::Synchronizer > sync_; - message_filters::Subscriber sub_image_; - message_filters::Subscriber sub_scan_; - - ros::Subscriber sub_camera_info_; - ros::Subscriber sub_detection_start_; - ros::Publisher pub_target_image_; - ros::Publisher pub_target_direction_; - ros::Publisher pub_target_image_center_; - boost::shared_ptr > srv_; - - /* rosparam */ - string image_topic_name_; - string camera_info_topic_name_; - string laser_scan_topic_name_; - string target_image_topic_name_; - string target_direction_topic_name_; - string target_image_center_topic_name_; - string detection_start_topic_name_; - - bool detect_once_; - bool verbose_; - - int hsv_lower_bound_[3]; - int hsv_upper_bound_[3]; - int contour_color_r_, contour_color_g_, contour_color_b_; - cv::Scalar contour_color_; - double color_region_tree_clustering_angle_diff_thre_; - double first_detection_depth_thre_; - - /* base var */ - double camera_fx_, camera_fy_, camera_cx_, camera_cy_; - - virtual void onInit(); - void configCallback(Config &new_config, uint32_t level); - - void cameraScanCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg); - void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info); - void startDetectionCallback(const std_msgs::BoolConstPtr& msg); - - void subscribe() - { - sub_image_.subscribe(); //start - sub_scan_.subscribe(); //start - } - - void unsubscribe() - { - sub_image_.unsubscribe(); //stop - sub_scan_.unsubscribe(); //stop - } - - }; -} - -#endif diff --git a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h new file mode 100644 index 0000000..5474a10 --- /dev/null +++ b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h @@ -0,0 +1,223 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + + +#ifndef VISION_DETECTOR_BASE_PLUGIN_H_ +#define VISION_DETECTOR_BASE_PLUGIN_H_ + +/* ros */ +#include +#include + +/* message filter */ +#include +#include +#include + +/* cfg */ +#include +#include + +/* ros msg */ +#include +#include +#include +#include +#include +#include + +/* cv */ +#include +#include +#include + +/* standard */ +#include +#include + +using namespace std; + +namespace vision_detection +{ + typedef message_filters::sync_policies::ApproximateTime SyncPolicy; + + class DetectorBase + { + public: + DetectorBase(): + camera_fx_(-1), camera_fy_(-1), camera_cx_(-1), camera_cy_(-1), + scan_angle_increment_(0), scan_angle_min_(0) + { + } + + ~DetectorBase(){} + + virtual void initialize(ros::NodeHandle nh, ros::NodeHandle pnh) + { + nh_ = nh; + pnh_ = pnh; + + /* ros param */ + pnh_.param("image_topic_name", image_topic_name_, std::string("/camera/image_rect")); + pnh_.param("camera_info_topic_name", camera_info_topic_name_, std::string("camera_info")); + pnh_.param("laser_scan_topic_name", laser_scan_topic_name_, std::string("/scan")); + pnh_.param("target_image_topic_name", target_image_topic_name_, std::string("/target")); + pnh_.param("detection_start_topic_name", detection_start_topic_name_, string("/detection_start")); + pnh_.param("target_image_center_topic_name", target_image_center_topic_name_, std::string("/object_image_center")); + pnh_.param("target_direction_topic_name", target_direction_topic_name_, std::string("/object_direction")); + + pnh_.param("detect_once", detect_once_, true); + pnh_.param("verbose", verbose_, true); + + pnh_.param("first_detection_depth_thre", first_detection_depth_thre_, 9.0); // [m] + + /* ros pub and sub */ + sub_image_.subscribe(nh_, image_topic_name_, 1); + sub_scan_.subscribe(nh_, laser_scan_topic_name_, 1); + unsubscribe(); + sync_ = boost::shared_ptr >(new message_filters::Synchronizer(SyncPolicy(10))); + sync_->connectInput(sub_image_, sub_scan_); + sync_->registerCallback(&DetectorBase::cameraScanCallback, this); + + sub_detection_start_ = nh_.subscribe(detection_start_topic_name_, 1, &DetectorBase::startDetectionCallback, this); + sub_camera_info_ = nh_.subscribe(camera_info_topic_name_, 1, &DetectorBase::cameraInfoCallback, this); + + pub_target_image_ = nh_.advertise(target_image_topic_name_, 1); + pub_target_direction_ = nh_.advertise("/object_direction", 1); + pub_target_image_center_ = nh_.advertise("/object_image_center", 1); + } + + protected: + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + boost::shared_ptr< message_filters::Synchronizer > sync_; + message_filters::Subscriber sub_image_; + message_filters::Subscriber sub_scan_; + + ros::Subscriber sub_camera_info_; + ros::Subscriber sub_detection_start_; + ros::Publisher pub_target_image_; + ros::Publisher pub_target_direction_; + ros::Publisher pub_target_image_center_; + + /* rosparam */ + string image_topic_name_; + string camera_info_topic_name_; + string laser_scan_topic_name_; + string target_image_topic_name_; + string target_direction_topic_name_; + string target_image_center_topic_name_; + string detection_start_topic_name_; + + bool detect_once_; + bool verbose_; + + double first_detection_depth_thre_; + + /* base var */ + float scan_angle_increment_, scan_angle_min_; + double camera_fx_, camera_fy_, camera_cx_, camera_cy_; + + void cameraScanCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg) + { + /* debug */ + ROS_WARN("camera scan vs laser scan time diff: %f", (image_msg->header.stamp.toSec() - scan_msg->header.stamp.toSec())); + + /* check whether the camera info is updated */ + if(camera_fx_ < -1) return; + + /* do the specific filter process */ + int target_tree_index = 0; + if(!filter(image_msg, scan_msg, target_tree_index)) return; + + /* publish the result */ + geometry_msgs::Vector3Stamped object_direction; + object_direction.header = scan_msg->header; + object_direction.vector.x = target_tree_index; //index + object_direction.vector.y = target_tree_index * scan_msg->angle_increment + scan_msg->angle_min; //direction + object_direction.vector.z = scan_msg->ranges[target_tree_index]; //distance + pub_target_direction_.publish(object_direction); + + /* stop the subscribe if necessary */ + if(detect_once_) unsubscribe(); + } + + void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info) + { + camera_fx_ = camera_info->K[0]; + camera_fy_ = camera_info->K[4]; + camera_cx_ = camera_info->K[2]; + camera_cy_ = camera_info->K[5]; + } + void startDetectionCallback(const std_msgs::BoolConstPtr& msg) + { + if(msg->data) + { + ROS_INFO("start detection"); + subscribe(); + } + else + { + ROS_INFO("stop perception"); + unsubscribe(); + } + + } + + void subscribe() + { + sub_image_.subscribe(); //start + sub_scan_.subscribe(); //start + } + + void unsubscribe() + { + sub_image_.unsubscribe(); //stop + sub_scan_.unsubscribe(); //stop + } + + void laserClustering(sensor_msgs::LaserScan scan_in, vector& cluster_index) + { + cluster_index.resize(0); + /* extract the cluster */ + for (size_t i = 0; i < scan_in.ranges.size(); i++) + if(scan_in.ranges[i] > 0) cluster_index.push_back(i); + } + + virtual bool filter(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg, int& target_tree_index) = 0; + }; +} + +#endif diff --git a/jsk_uav_forest_perception/launch/image_process.launch b/jsk_uav_forest_perception/launch/image_process.launch index 548a947..a773449 100644 --- a/jsk_uav_forest_perception/launch/image_process.launch +++ b/jsk_uav_forest_perception/launch/image_process.launch @@ -1,27 +1,19 @@ - - + - - - - - - - - - - - - - - - - - + + + + + + + + + + diff --git a/jsk_uav_forest_perception/launch/laser_process.launch b/jsk_uav_forest_perception/launch/laser_process.launch index 2ef07ba..473eb68 100644 --- a/jsk_uav_forest_perception/launch/laser_process.launch +++ b/jsk_uav_forest_perception/launch/laser_process.launch @@ -11,7 +11,7 @@ - + diff --git a/jsk_uav_forest_perception/launch/tree_detector.launch b/jsk_uav_forest_perception/launch/tree_detector.launch index 7b2b9e4..aa4b618 100644 --- a/jsk_uav_forest_perception/launch/tree_detector.launch +++ b/jsk_uav_forest_perception/launch/tree_detector.launch @@ -6,7 +6,6 @@ - @@ -20,5 +19,5 @@ - + diff --git a/jsk_uav_forest_perception/package.xml b/jsk_uav_forest_perception/package.xml index 95edddd..b1996c7 100644 --- a/jsk_uav_forest_perception/package.xml +++ b/jsk_uav_forest_perception/package.xml @@ -43,7 +43,7 @@ tf - + diff --git a/jsk_uav_forest_perception/param/IndependentHsvFilter.yaml b/jsk_uav_forest_perception/param/IndependentHsvFilter.yaml new file mode 100644 index 0000000..a985cd3 --- /dev/null +++ b/jsk_uav_forest_perception/param/IndependentHsvFilter.yaml @@ -0,0 +1,12 @@ +# Independent HSV Filter + +## HSV param +h_min: 150 +s_min: 80 +v_min: 100 +h_max: 16 +s_max: 255 +v_max: 255 + +## Laser param +first_detection_depth_thre: 9.0 \ No newline at end of file diff --git a/jsk_uav_forest_perception/param/TreeFilter.yaml b/jsk_uav_forest_perception/param/LaserClustering.yaml similarity index 100% rename from jsk_uav_forest_perception/param/TreeFilter.yaml rename to jsk_uav_forest_perception/param/LaserClustering.yaml diff --git a/jsk_uav_forest_perception/param/VisionDetection.yaml b/jsk_uav_forest_perception/param/VisionDetection.yaml deleted file mode 100644 index 3c037df..0000000 --- a/jsk_uav_forest_perception/param/VisionDetection.yaml +++ /dev/null @@ -1,8 +0,0 @@ -camera: - vision_detection: - h_min: 150 - s_min: 80 - v_min: 100 - h_max: 16 - s_max: 255 - v_max: 255 \ No newline at end of file diff --git a/jsk_uav_forest_perception/src/vision_detection.cpp b/jsk_uav_forest_perception/src/vision_detection.cpp new file mode 100644 index 0000000..9e2fdda --- /dev/null +++ b/jsk_uav_forest_perception/src/vision_detection.cpp @@ -0,0 +1,83 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* base class */ +#include +#include + +namespace vision_detection +{ + class VisionDetection + { + public: + VisionDetection(ros::NodeHandle nh, ros::NodeHandle pnh): + nh_(nh), pnh_(pnh) + { + + detector_plugin_ptr_ = boost::shared_ptr< pluginlib::ClassLoader >( new pluginlib::ClassLoader("jsk_uav_forest_perception", "vision_detection::DetectorBase")); + + std::string plugin_name; + if(pnh_.getParam("plugin_name", plugin_name)) + { + detector_ = detector_plugin_ptr_->createInstance(plugin_name); + detector_->initialize(nh, pnh); + } + else + ROS_ERROR("%s: can not get the plugin name", pnh_.getNamespace().c_str()); + } + + ~VisionDetection (){}; + + private: + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + boost::shared_ptr detector_; + boost::shared_ptr< pluginlib::ClassLoader > detector_plugin_ptr_; + + }; +}; + + +int main (int argc, char **argv) +{ + ros::init (argc, argv, "vision_detection"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + vision_detection::VisionDetection* vision_detection_node = new vision_detection::VisionDetection(nh, pnh); + ros::spin (); + delete vision_detection_node; + return 0; +} diff --git a/jsk_uav_forest_perception/src/vision_detector.cpp b/jsk_uav_forest_perception/src/vision_detector.cpp deleted file mode 100644 index 1f4a2a9..0000000 --- a/jsk_uav_forest_perception/src/vision_detector.cpp +++ /dev/null @@ -1,285 +0,0 @@ -// -*- mode: c++ -*- -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, JSK Lab - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/o2r other materials provided - * with the distribution. - * * Neither the name of the JSK Lab nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - - -#include "jsk_uav_forest_perception/vision_detector.h" - -namespace vision_detection -{ - VisionDetector::VisionDetector(): - camera_fx_(-1), camera_fy_(-1), camera_cx_(-1), camera_cy_(-1) - { - } - - VisionDetector::~VisionDetector() - { - } - - void VisionDetector::onInit() - { - pnh_ = this->getPrivateNodeHandle(); - - /* ros param */ - pnh_.param("image_topic_name", image_topic_name_, std::string("/camera/image_rect")); - pnh_.param("camera_info_topic_name", camera_info_topic_name_, std::string("camera_info")); - pnh_.param("laser_scan_topic_name", laser_scan_topic_name_, std::string("/scan")); - pnh_.param("target_image_topic_name", target_image_topic_name_, std::string("/target")); - pnh_.param("detection_start_topic_name", detection_start_topic_name_, string("/detection_start")); - pnh_.param("target_image_center_topic_name", target_image_center_topic_name_, std::string("/object_image_center")); - pnh_.param("target_direction_topic_name", target_direction_topic_name_, std::string("/object_direction")); - - pnh_.param("detect_once", detect_once_, true); - pnh_.param("verbose", verbose_, true); - - pnh_.param("color_region_tree_clustering_angle_diff_thre", color_region_tree_clustering_angle_diff_thre_, 0.10); - pnh_.param("first_detection_depth_thre", first_detection_depth_thre_, 9.0); // [m] - - pnh_.param("h_min", hsv_lower_bound_[0], 0); - pnh_.param("s_min", hsv_lower_bound_[1], 0); - pnh_.param("v_min", hsv_lower_bound_[2], 0); - pnh_.param("h_max", hsv_upper_bound_[0], 255); - pnh_.param("s_max", hsv_upper_bound_[1], 255); - pnh_.param("v_max", hsv_upper_bound_[2], 255); - - pnh_.param("contour_color_r", contour_color_r_, 255); - pnh_.param("contour_color_g", contour_color_g_, 255); - pnh_.param("contour_color_b", contour_color_b_, 255); - contour_color_ = cv::Scalar(contour_color_r_, contour_color_g_, contour_color_b_); - - /* ros pub and sub */ - sub_image_.subscribe(pnh_, image_topic_name_, 1); - sub_scan_.subscribe(pnh_, laser_scan_topic_name_, 1); - unsubscribe(); - sync_ = boost::shared_ptr >(new message_filters::Synchronizer(SyncPolicy(10))); - sync_->connectInput(sub_image_, sub_scan_); - sync_->registerCallback(&VisionDetector::cameraScanCallback, this); - - sub_detection_start_ = pnh_.subscribe(detection_start_topic_name_, 1, &VisionDetector::startDetectionCallback, this); - sub_camera_info_ = pnh_.subscribe(camera_info_topic_name_, 1, &VisionDetector::cameraInfoCallback, this); - - pub_target_image_ = pnh_.advertise(target_image_topic_name_, 1); - pub_target_direction_ = pnh_.advertise("/object_direction", 1); - pub_target_image_center_ = pnh_.advertise("/object_image_center", 1); - - /* ros service */ - srv_ = boost::make_shared > (pnh_); - dynamic_reconfigure::Server::CallbackType f = - boost::bind (&VisionDetector::configCallback, this, _1, _2); - srv_->setCallback (f); - } - - void VisionDetector::configCallback(Config &new_config, uint32_t level) - { - if(!new_config.enable) return; - - ROS_WARN("New config"); - - hsv_lower_bound_[0] = new_config.color_hsv_h_min; - hsv_upper_bound_[0] = new_config.color_hsv_h_max; - hsv_lower_bound_[1] = new_config.color_hsv_s_min; - hsv_upper_bound_[1] = new_config.color_hsv_s_max; - hsv_lower_bound_[2] = new_config.color_hsv_v_min; - hsv_upper_bound_[2] = new_config.color_hsv_v_max; - } - - void VisionDetector::startDetectionCallback(const std_msgs::BoolConstPtr& msg) - { - if(msg->data) - { - ROS_INFO("start detection"); - subscribe(); - } - else - { - ROS_INFO("stop perception"); - unsubscribe(); - } - } - - void VisionDetector::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info) - { - camera_fx_ = camera_info->K[0]; - camera_fy_ = camera_info->K[4]; - camera_cx_ = camera_info->K[2]; - camera_cy_ = camera_info->K[5]; - } - - void VisionDetector::cameraScanCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg) - { - /* debug */ - ROS_WARN("camera scan vs laser scan time diff: %f", (image_msg->header.stamp.toSec() - scan_msg->header.stamp.toSec())); - - /* laser part */ - vector cluster_index; - /* extract the cluster */ - for (size_t i = 0; i < scan_msg->ranges.size(); i++) - if(scan_msg->ranges[i] > 0) cluster_index.push_back(i); - - /* vision part */ - /* check whether the camera info is updated */ - if(camera_fx_ < -1) return; - - cv::Mat src_image = cv_bridge::toCvCopy(image_msg, image_msg->encoding)->image; - - /* independent hsv filter */ - cv::Mat hsv_image, hsv_image_mask; - if(image_msg->encoding == sensor_msgs::image_encodings::RGB8) - cv::cvtColor(src_image, hsv_image, CV_RGB2HSV); - else if(image_msg->encoding == sensor_msgs::image_encodings::BGR8) - cv::cvtColor(src_image, hsv_image, CV_BGR2HSV); - - if(hsv_lower_bound_[0] < hsv_upper_bound_[0]) - { - cv::inRange(hsv_image, - cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]), - cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]), - hsv_image_mask); - } - else - { - cv::Scalar lower_color_range_0 = cv::Scalar(0, hsv_lower_bound_[1], hsv_lower_bound_[2]); - cv::Scalar upper_color_range_0 = cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]); - cv::Scalar lower_color_range_360 = cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]); - cv::Scalar upper_color_range_360 = cv::Scalar(180, hsv_upper_bound_[1], hsv_upper_bound_[2]); - cv::Mat output_image_0, output_image_360; - cv::inRange(hsv_image, lower_color_range_0, upper_color_range_0, output_image_0); - cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360); - hsv_image_mask = output_image_0 | output_image_360; - } - - /* find countour */ - std::vector > contours; - std::vector hierarchy; - cv::findContours(hsv_image_mask, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cv::Point(0,0)); - - if(contours.size() == 0) return; - - /* find the biggest contour */ - float max_contour_area = 0; - float max_contour = 0; - for(int i = 0; i < contours.size(); ++i) - { - if (cv::contourArea(contours[i]) > max_contour_area) - { - max_contour_area = cv::contourArea(contours[i]); - max_contour = i; - } - } - cv::Moments contour_moments = cv::moments(contours[max_contour], true); - - /* publish the base information */ - geometry_msgs::PointStamped contour_center; - contour_center.header = image_msg->header; - contour_center.point.x = contour_moments.m10 / contour_moments.m00; - contour_center.point.y = contour_moments.m01 / contour_moments.m00; - pub_target_image_center_.publish(contour_center); - /* draw */ - cv::circle(src_image, cv::Point(contour_center.point.x, contour_center.point.y), 3, cv::Scalar(255, 0, 0), 5); - cv::drawContours(src_image, contours, max_contour, contour_color_, 3); - - /* publish */ - pub_target_image_.publish(cv_bridge::CvImage(image_msg->header, - image_msg->encoding, - src_image).toImageMsg()); - - - /* check whether the contour is satisfied the condistion */ - - /* we calculate the angle between the x axis(forward) and the line to the color region */ - float color_region_direction = atan2(-contour_center.point.x + camera_cx_, camera_fx_); - if(verbose_) cout << "vision detection direction" << color_region_direction << endl; - - /* Find the most closed tree to the color region, record the target tree index */ - float fov = atan2(camera_cx_, camera_fx_); //proximate the width of image with camera_cx_ - float min_diff = 1e6; - int target_tree_index = 0; - for ( vector::iterator it = cluster_index.begin(); it != cluster_index.end(); ++it) - { - float diff = 0; - - float laser_direction = *it * scan_msg->angle_increment + scan_msg->angle_min; - if(fabs(laser_direction) > fov) - { - if(verbose_) cout << "eliminate: laser_direction:" << laser_direction << "; " << "fov: " << fov << endl; - continue; - } - - if(scan_msg->ranges[*it] > first_detection_depth_thre_) - { - if(verbose_) cout << "eliminate: depth: " << scan_msg->ranges[*it] << endl; - continue; - } - - diff = fabs(laser_direction - color_region_direction); - - if(verbose_) - { - cout << "tree index: " << *it << endl; - cout << "tree angle: " << *it * scan_msg->angle_increment + scan_msg->angle_min << endl; - cout << "diff: " << diff << endl; - } - - if(diff < min_diff) - { - min_diff = diff; - target_tree_index = *it; - } - } - - /* Check whether the tree and color region is closed enough */ - if(min_diff > color_region_tree_clustering_angle_diff_thre_) - { - ROS_INFO_THROTTLE(1,"can not find the target tree, diff: %f", min_diff); - return; - } - - - /* find the target tree */ - ROS_WARN("vision detectoion: find the target tree, angle_diff: %f", min_diff); - - geometry_msgs::Vector3Stamped object_direction; - object_direction.header = scan_msg->header; - object_direction.vector.x = target_tree_index; //index - object_direction.vector.y = target_tree_index * scan_msg->angle_increment + scan_msg->angle_min; //direction - object_direction.vector.z = scan_msg->ranges[target_tree_index]; //distance - pub_target_direction_.publish(object_direction); - - /* stop the subscribe if necessary */ - if(detect_once_) unsubscribe(); - } -} - -#include -PLUGINLIB_EXPORT_CLASS (vision_detection::VisionDetector, nodelet::Nodelet); diff --git a/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp b/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp new file mode 100644 index 0000000..ae2ffee --- /dev/null +++ b/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp @@ -0,0 +1,228 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* base class */ +#include + +using namespace std; + +namespace vision_detection +{ + typedef jsk_uav_forest_perception::HsvFilterConfig Config; + class IndependentHsvFilter :public vision_detection::DetectorBase + { + public: + void initialize(ros::NodeHandle nh, ros::NodeHandle pnh) + { + vision_detection::DetectorBase::initialize(nh, pnh); + + pnh_.param("color_region_tree_clustering_angle_diff_thre", color_region_tree_clustering_angle_diff_thre_, 0.10); + + pnh_.param("h_min", hsv_lower_bound_[0], 0); + pnh_.param("s_min", hsv_lower_bound_[1], 0); + pnh_.param("v_min", hsv_lower_bound_[2], 0); + pnh_.param("h_max", hsv_upper_bound_[0], 255); + pnh_.param("s_max", hsv_upper_bound_[1], 255); + pnh_.param("v_max", hsv_upper_bound_[2], 255); + + pnh_.param("contour_color_r", contour_color_r_, 255); + pnh_.param("contour_color_g", contour_color_g_, 255); + pnh_.param("contour_color_b", contour_color_b_, 255); + contour_color_ = cv::Scalar(contour_color_r_, contour_color_g_, contour_color_b_); + + + /* ros service */ + srv_ = boost::make_shared > (nh_); + dynamic_reconfigure::Server::CallbackType f = + boost::bind (&IndependentHsvFilter::configCallback, this, _1, _2); + srv_->setCallback (f); + + } + private: + boost::shared_ptr > srv_; + + int hsv_lower_bound_[3]; + int hsv_upper_bound_[3]; + int contour_color_r_, contour_color_g_, contour_color_b_; + cv::Scalar contour_color_; + double color_region_tree_clustering_angle_diff_thre_; + + void configCallback(Config &new_config, uint32_t level) + { + if(!new_config.enable) return; + + ROS_WARN("New config"); + + hsv_lower_bound_[0] = new_config.color_hsv_h_min; + hsv_upper_bound_[0] = new_config.color_hsv_h_max; + hsv_lower_bound_[1] = new_config.color_hsv_s_min; + hsv_upper_bound_[1] = new_config.color_hsv_s_max; + hsv_lower_bound_[2] = new_config.color_hsv_v_min; + hsv_upper_bound_[2] = new_config.color_hsv_v_max; + } + + + bool filter(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg, int& target_tree_index) + { + cv::Mat src_image = cv_bridge::toCvCopy(image_msg, image_msg->encoding)->image; + + cv::Mat hsv_image, hsv_image_mask; + if(image_msg->encoding == sensor_msgs::image_encodings::RGB8) + cv::cvtColor(src_image, hsv_image, CV_RGB2HSV); + else if(image_msg->encoding == sensor_msgs::image_encodings::BGR8) + cv::cvtColor(src_image, hsv_image, CV_BGR2HSV); + + if(hsv_lower_bound_[0] < hsv_upper_bound_[0]) + { + cv::inRange(hsv_image, + cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]), + cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]), + hsv_image_mask); + } + else + { + cv::Scalar lower_color_range_0 = cv::Scalar(0, hsv_lower_bound_[1], hsv_lower_bound_[2]); + cv::Scalar upper_color_range_0 = cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]); + cv::Scalar lower_color_range_360 = cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]); + cv::Scalar upper_color_range_360 = cv::Scalar(180, hsv_upper_bound_[1], hsv_upper_bound_[2]); + cv::Mat output_image_0, output_image_360; + cv::inRange(hsv_image, lower_color_range_0, upper_color_range_0, output_image_0); + cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360); + hsv_image_mask = output_image_0 | output_image_360; + } + + /* find countour */ + std::vector > contours; + std::vector hierarchy; + cv::findContours(hsv_image_mask, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cv::Point(0,0)); + + if(contours.size() == 0) return false; + + /* find the biggest contour */ + float max_contour_area = 0; + float max_contour = 0; + for(int i = 0; i < contours.size(); ++i) + { + if (cv::contourArea(contours[i]) > max_contour_area) + { + max_contour_area = cv::contourArea(contours[i]); + max_contour = i; + } + } + cv::Moments contour_moments = cv::moments(contours[max_contour], true); + + /* publish the base information */ + geometry_msgs::PointStamped contour_center; + contour_center.header = image_msg->header; + contour_center.point.x = contour_moments.m10 / contour_moments.m00; + contour_center.point.y = contour_moments.m01 / contour_moments.m00; + pub_target_image_center_.publish(contour_center); + /* draw */ + cv::circle(src_image, cv::Point(contour_center.point.x, contour_center.point.y), 3, cv::Scalar(255, 0, 0), 5); + cv::drawContours(src_image, contours, max_contour, contour_color_, 3); + + /* publish */ + pub_target_image_.publish(cv_bridge::CvImage(image_msg->header, + image_msg->encoding, + src_image).toImageMsg()); + + + /* check whether the contour is satisfied the condistion */ + + /* we calculate the angle between the x axis(forward) and the line to the color region */ + float color_region_direction = atan2(-contour_center.point.x + camera_cx_, camera_fx_); + if(verbose_) cout << "vision detection direction" << color_region_direction << endl; + + /* Find the most closed tree to the color region, record the target tree index */ + float fov = atan2(camera_cx_, camera_fx_); //proximate the width of image with camera_cx_ + float min_diff = 1e6; + + vector cluster_index; + laserClustering(*scan_msg, cluster_index); + + for ( vector::iterator it = cluster_index.begin(); it != cluster_index.end(); ++it) + { + float diff = 0; + float laser_direction = *it * scan_msg->angle_increment + scan_msg->angle_min; + + if(fabs(laser_direction) > fov) + { + if(verbose_) cout << "eliminate: laser_direction:" << laser_direction << "; " << "fov: " << fov << endl; + continue; + } + + if(scan_msg->ranges[*it] > first_detection_depth_thre_) + { + if(verbose_) cout << "eliminate: depth: " << scan_msg->ranges[*it] << endl; + continue; + } + + diff = fabs(laser_direction - color_region_direction); + + if(verbose_) + { + cout << "tree index: " << *it << endl; + cout << "tree angle: " << laser_direction << endl; + cout << "diff: " << diff << endl; + } + + if(diff < min_diff) + { + min_diff = diff; + target_tree_index = *it; + } + + } + + /* Check whether the tree and color region is closed enough */ + if(min_diff > color_region_tree_clustering_angle_diff_thre_) + { + ROS_INFO_THROTTLE(1,"closest tree is far from the color region, can not find the target tree, diff: %f", min_diff); + return false; + } + + /* find the target tree */ + ROS_WARN("vision detectoion: find the target tree, angle_diff: %f", min_diff); + return true; + } + }; + +}; + + +/* plugin registration */ +#include +PLUGINLIB_EXPORT_CLASS(vision_detection::IndependentHsvFilter, vision_detection::DetectorBase); + diff --git a/jsk_uav_forest_perception/vision_detection_nodelets.xml b/jsk_uav_forest_perception/vision_detection_nodelets.xml deleted file mode 100644 index b51646a..0000000 --- a/jsk_uav_forest_perception/vision_detection_nodelets.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Nodelet for vision detection. - - - diff --git a/jsk_uav_forest_perception/vision_detection_plugins.xml b/jsk_uav_forest_perception/vision_detection_plugins.xml new file mode 100644 index 0000000..b306398 --- /dev/null +++ b/jsk_uav_forest_perception/vision_detection_plugins.xml @@ -0,0 +1,7 @@ + + + + Independent HSV filter. + + + From 353d4dc49f93705912d55fb51505a5177bb0832f Mon Sep 17 00:00:00 2001 From: tongtybj Date: Mon, 24 Apr 2017 01:22:13 +0900 Subject: [PATCH 04/10] Increate the queue size and add the hints for synchronized messages in tree tracking which gathers the target tree index from vision detection and tree cluster scan message. This solves the problem that sometimes the synchroinzed message is not called. --- jsk_uav_forest_perception/src/tree_tracking.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/jsk_uav_forest_perception/src/tree_tracking.cpp b/jsk_uav_forest_perception/src/tree_tracking.cpp index e486724..ed50b0b 100644 --- a/jsk_uav_forest_perception/src/tree_tracking.cpp +++ b/jsk_uav_forest_perception/src/tree_tracking.cpp @@ -54,10 +54,10 @@ TreeTracking::TreeTracking(ros::NodeHandle nh, ros::NodeHandle nhp): nhp_.param("uav_tilt_thre", uav_tilt_thre_, 0.17); nhp_.param("verbose", verbose_, false); - sub_sync_scan_.subscribe(nhp_, laser_scan_topic_name_, 1); - sub_sync_vision_detection_.subscribe(nhp_, vision_detection_topic_name_, 1); + sub_sync_scan_.subscribe(nhp_, laser_scan_topic_name_, 20, ros::TransportHints().tcpNoDelay()); + sub_sync_vision_detection_.subscribe(nhp_, vision_detection_topic_name_, 10, ros::TransportHints().tcpNoDelay()); - sync_ = boost::shared_ptr(new SyncPolicy(sub_sync_scan_, sub_sync_vision_detection_, 10)); + sync_ = boost::shared_ptr(new SyncPolicy(sub_sync_scan_, sub_sync_vision_detection_, 100)); sync_->registerCallback(&TreeTracking::visionDetectionCallback, this); sub_uav_odom_ = nh_.subscribe(uav_odom_topic_name_, 1, &TreeTracking::uavOdomCallback, this); From 970f2df643d9c61a67241f251c671444ab8b1d50 Mon Sep 17 00:00:00 2001 From: tongtybj Date: Mon, 24 Apr 2017 01:24:55 +0900 Subject: [PATCH 05/10] Update the gazebo UAV model to be the exact same with real machine. --- .../urdf/quadrotor_cam_hokuyo.gazebo.xacro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_uav_forest_simulation/urdf/quadrotor_cam_hokuyo.gazebo.xacro b/jsk_uav_forest_simulation/urdf/quadrotor_cam_hokuyo.gazebo.xacro index 8ae83a6..2cd291c 100644 --- a/jsk_uav_forest_simulation/urdf/quadrotor_cam_hokuyo.gazebo.xacro +++ b/jsk_uav_forest_simulation/urdf/quadrotor_cam_hokuyo.gazebo.xacro @@ -35,8 +35,8 @@ - - + + From 9fe8d52669fc2bd9952957b7d97b01219e3ddb6d Mon Sep 17 00:00:00 2001 From: tongtybj Date: Mon, 24 Apr 2017 01:26:06 +0900 Subject: [PATCH 06/10] Add conditional HSV filter plugin --- jsk_uav_forest_perception/CMakeLists.txt | 3 +- .../vision_detector/conditional_hsv_filter.h | 54 +++ .../vision_detector/independent_hsv_filter.h | 65 ++++ .../vision_detector_base_plugin.h | 28 +- .../param/ConditionalHsvFilter.yaml | 12 + .../conditional_hsv_filter.cpp | 155 ++++++++ .../independent_hsv_filter.cpp | 345 +++++++++--------- .../vision_detection_plugins.xml | 6 + 8 files changed, 490 insertions(+), 178 deletions(-) create mode 100644 jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector/conditional_hsv_filter.h create mode 100644 jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector/independent_hsv_filter.h create mode 100644 jsk_uav_forest_perception/param/ConditionalHsvFilter.yaml create mode 100644 jsk_uav_forest_perception/src/vision_detector/conditional_hsv_filter.cpp diff --git a/jsk_uav_forest_perception/CMakeLists.txt b/jsk_uav_forest_perception/CMakeLists.txt index 004c063..9eea249 100644 --- a/jsk_uav_forest_perception/CMakeLists.txt +++ b/jsk_uav_forest_perception/CMakeLists.txt @@ -44,7 +44,8 @@ add_library(laser_clustering_filter SHARED src/laser_clustering_filter.cpp) target_link_libraries(laser_clustering_filter ${catkin_LIBRARIES}) add_library(vision_detection_pluginlib - src/vision_detector/independent_hsv_filter.cpp) + src/vision_detector/independent_hsv_filter.cpp + src/vision_detector/conditional_hsv_filter.cpp) target_link_libraries(vision_detection_pluginlib ${catkin_LIBRARIES}) add_dependencies(vision_detection_pluginlib ${PROJECT_NAME}_gencfg) diff --git a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector/conditional_hsv_filter.h b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector/conditional_hsv_filter.h new file mode 100644 index 0000000..901091d --- /dev/null +++ b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector/conditional_hsv_filter.h @@ -0,0 +1,54 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* base class */ +#include + + +using namespace std; + +namespace vision_detection +{ + class ConditionalHsvFilter :public vision_detection::IndependentHsvFilter + { + public: + virtual void initialize(ros::NodeHandle nh, ros::NodeHandle pnh); + protected: + double tree_diameter_; + + virtual bool filter(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg, int& target_tree_index); + }; + +}; diff --git a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector/independent_hsv_filter.h b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector/independent_hsv_filter.h new file mode 100644 index 0000000..8715a92 --- /dev/null +++ b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector/independent_hsv_filter.h @@ -0,0 +1,65 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* base class */ +#include + +using namespace std; + +namespace vision_detection +{ + typedef jsk_uav_forest_perception::HsvFilterConfig Config; + class IndependentHsvFilter :public vision_detection::DetectorBase + { + public: + virtual void initialize(ros::NodeHandle nh, ros::NodeHandle pnh); + protected: + boost::shared_ptr > srv_; + + int hsv_lower_bound_[3]; + int hsv_upper_bound_[3]; + int contour_color_r_, contour_color_g_, contour_color_b_; + cv::Scalar contour_color_; + double color_region_tree_clustering_angle_diff_thre_; + + void configCallback(Config &new_config, uint32_t level); + + void hsvFilter(cv::Mat src_image, string encoding, cv::Mat& hue_image_mask); + + virtual bool filter(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg, int& target_tree_index); + }; + +}; + diff --git a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h index 5474a10..b9878db 100644 --- a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h +++ b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h @@ -208,12 +208,36 @@ namespace vision_detection sub_scan_.unsubscribe(); //stop } - void laserClustering(sensor_msgs::LaserScan scan_in, vector& cluster_index) + void laserClustering(sensor_msgs::LaserScan scan_in, vector& cluster_index, cv::Mat src_image) { cluster_index.resize(0); /* extract the cluster */ for (size_t i = 0; i < scan_in.ranges.size(); i++) - if(scan_in.ranges[i] > 0) cluster_index.push_back(i); + { + if(scan_in.ranges[i] > 0) + { + cluster_index.push_back(i); + + float laser_direction = i * scan_in.angle_increment + scan_in.angle_min; + cv::circle(src_image, cv::Point(camera_cx_ - camera_fx_ * tan(laser_direction) , camera_cy_), 3, cv::Scalar(255, 0, 0), 5); + } + } + } + + bool commonFiltering(float laser_direction, float fov, float laser_distance) + { + if(fabs(laser_direction) > fov) + { + if(verbose_) cout << "eliminate: laser_direction:" << laser_direction << "; " << "fov: " << fov << endl; + return false; + } + + if(laser_distance > first_detection_depth_thre_) + { + if(verbose_) cout << "eliminate: depth: " << laser_distance << endl; + return false; + } + return true; } virtual bool filter(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg, int& target_tree_index) = 0; diff --git a/jsk_uav_forest_perception/param/ConditionalHsvFilter.yaml b/jsk_uav_forest_perception/param/ConditionalHsvFilter.yaml new file mode 100644 index 0000000..a21f532 --- /dev/null +++ b/jsk_uav_forest_perception/param/ConditionalHsvFilter.yaml @@ -0,0 +1,12 @@ +# Conditinal HSV Filter + +## HSV param +h_min: 150 +s_min: 80 +v_min: 100 +h_max: 16 +s_max: 255 +v_max: 255 + +## Laser param +tree_diameter: 0.3 \ No newline at end of file diff --git a/jsk_uav_forest_perception/src/vision_detector/conditional_hsv_filter.cpp b/jsk_uav_forest_perception/src/vision_detector/conditional_hsv_filter.cpp new file mode 100644 index 0000000..a8da0ac --- /dev/null +++ b/jsk_uav_forest_perception/src/vision_detector/conditional_hsv_filter.cpp @@ -0,0 +1,155 @@ +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* base class */ +#include + +using namespace std; + +namespace vision_detection +{ + void ConditionalHsvFilter::initialize(ros::NodeHandle nh, ros::NodeHandle pnh) + { + vision_detection::IndependentHsvFilter::initialize(nh, pnh); + + pnh_.param("tree_diameter", tree_diameter_, 0.3); // should be smaller than real size + + } + + + bool ConditionalHsvFilter::filter(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg, int& target_tree_index) + { + cv::Mat src_image = cv_bridge::toCvCopy(image_msg, image_msg->encoding)->image; + + /* general hsv filter */ + cv::Mat hsv_image_mask; + hsvFilter(src_image, image_msg->encoding, hsv_image_mask); + + /* labeling, find the max contour inside each tree area, and choose the biggest one among the max contours. */ + vector cluster_index; + laserClustering(*scan_msg, cluster_index, src_image); + + float fov = atan2(camera_cx_, camera_fx_); //proximate the width of image with camera_cx_ + float max_contour_area = 0; + std::vector > max_contours; + int max_contour_id = 0; + cv::Mat target_tree_area; + int tree_index = 0; + for ( vector::iterator it = cluster_index.begin(); it != cluster_index.end(); ++it) + { + float laser_direction = *it * scan_msg->angle_increment + scan_msg->angle_min; + float laser_distance = scan_msg->ranges[*it]; + + if(!commonFiltering(laser_direction, fov, laser_distance)) continue; + + float x1 = laser_distance * cos(laser_direction) + tree_diameter_ / 2 * cos(laser_direction + M_PI/2); + float x2 = laser_distance * cos(laser_direction) + tree_diameter_ / 2 * cos(laser_direction - M_PI/2); + + float y1 = laser_distance * sin(laser_direction) + tree_diameter_ / 2 * sin(laser_direction - M_PI/2); + float y2 = laser_distance * sin(laser_direction) + tree_diameter_ / 2 * sin(laser_direction + M_PI/2); + float x_l = camera_cx_ - camera_fx_ * y2 / x2; + float x_r = camera_cx_ - camera_fx_ * y1 / x1; + if(x_l < 0) x_l = 0; + if(x_r > src_image.cols) x_r = src_image.cols; + if(verbose_) + ROS_INFO("roi calculation: x1:%f, x2:%f, y1:%f, y2:%f, x_l:%f, x_r:%f", x1, x2, y1, y2,x_l,x_r); + cv::Rect roi(x_l, 0, x_r - x_l, src_image.rows); + cv::Mat tree_area = hsv_image_mask(roi); + /* draw roi */ + cv::rectangle(src_image, roi, cv::Scalar(0, 0, 255), 3,8,0); + + float contour_area = 0; + std::vector > contours; + int contour_id = 0; + std::vector hierarchy; + cv::findContours(tree_area, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cv::Point(0,0)); + if(contours.size() == 0) + { + ROS_INFO("eliminate: no contour, tree index: %d, tree direction: %f", *it, laser_direction); + continue; + } + + /* find the max contour in roi */ + for(int i = 0; i < contours.size(); ++i) + { + if (cv::contourArea(contours[i]) > contour_area) + { + contour_area = cv::contourArea(contours[i]); + contour_id = i; + } + } + + /* update the biggest countour in all trees */ + if(contour_area > max_contour_area ) + { + max_contour_area = contour_area; + max_contour_id = contour_id; + max_contours = contours; + target_tree_index = *it; + tree_index = distance(cluster_index.begin(), it); + target_tree_area = src_image(roi); + } + + if(verbose_) + { + cout << "tree index: " << *it << endl; + cout << "tree angle: " << laser_direction << endl; + cout << "contour_area: " << contour_area << endl; + } + } + + if(max_contour_area == 0) return false; + + /* draw */ + cv::drawContours(target_tree_area, max_contours, max_contour_id, contour_color_, 5); + + /* publish */ + pub_target_image_.publish(cv_bridge::CvImage(image_msg->header, + image_msg->encoding, + src_image).toImageMsg()); + + + /* find the target tree */ + ROS_WARN("conditional hsv filter based vision detectoion: find the target tree."); + return true; + } + +}; + + +/* plugin registration */ +#include +PLUGINLIB_EXPORT_CLASS(vision_detection::ConditionalHsvFilter, vision_detection::DetectorBase); + diff --git a/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp b/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp index ae2ffee..dc36275 100644 --- a/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp +++ b/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp @@ -34,194 +34,189 @@ *********************************************************************/ /* base class */ -#include +#include using namespace std; namespace vision_detection { - typedef jsk_uav_forest_perception::HsvFilterConfig Config; - class IndependentHsvFilter :public vision_detection::DetectorBase + void IndependentHsvFilter::initialize(ros::NodeHandle nh, ros::NodeHandle pnh) { - public: - void initialize(ros::NodeHandle nh, ros::NodeHandle pnh) - { - vision_detection::DetectorBase::initialize(nh, pnh); - - pnh_.param("color_region_tree_clustering_angle_diff_thre", color_region_tree_clustering_angle_diff_thre_, 0.10); - - pnh_.param("h_min", hsv_lower_bound_[0], 0); - pnh_.param("s_min", hsv_lower_bound_[1], 0); - pnh_.param("v_min", hsv_lower_bound_[2], 0); - pnh_.param("h_max", hsv_upper_bound_[0], 255); - pnh_.param("s_max", hsv_upper_bound_[1], 255); - pnh_.param("v_max", hsv_upper_bound_[2], 255); - - pnh_.param("contour_color_r", contour_color_r_, 255); - pnh_.param("contour_color_g", contour_color_g_, 255); - pnh_.param("contour_color_b", contour_color_b_, 255); - contour_color_ = cv::Scalar(contour_color_r_, contour_color_g_, contour_color_b_); - - - /* ros service */ - srv_ = boost::make_shared > (nh_); - dynamic_reconfigure::Server::CallbackType f = - boost::bind (&IndependentHsvFilter::configCallback, this, _1, _2); - srv_->setCallback (f); - - } - private: - boost::shared_ptr > srv_; - - int hsv_lower_bound_[3]; - int hsv_upper_bound_[3]; - int contour_color_r_, contour_color_g_, contour_color_b_; - cv::Scalar contour_color_; - double color_region_tree_clustering_angle_diff_thre_; - - void configCallback(Config &new_config, uint32_t level) - { - if(!new_config.enable) return; - - ROS_WARN("New config"); - - hsv_lower_bound_[0] = new_config.color_hsv_h_min; - hsv_upper_bound_[0] = new_config.color_hsv_h_max; - hsv_lower_bound_[1] = new_config.color_hsv_s_min; - hsv_upper_bound_[1] = new_config.color_hsv_s_max; - hsv_lower_bound_[2] = new_config.color_hsv_v_min; - hsv_upper_bound_[2] = new_config.color_hsv_v_max; - } - - - bool filter(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg, int& target_tree_index) - { - cv::Mat src_image = cv_bridge::toCvCopy(image_msg, image_msg->encoding)->image; - - cv::Mat hsv_image, hsv_image_mask; - if(image_msg->encoding == sensor_msgs::image_encodings::RGB8) - cv::cvtColor(src_image, hsv_image, CV_RGB2HSV); - else if(image_msg->encoding == sensor_msgs::image_encodings::BGR8) - cv::cvtColor(src_image, hsv_image, CV_BGR2HSV); - - if(hsv_lower_bound_[0] < hsv_upper_bound_[0]) - { - cv::inRange(hsv_image, - cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]), - cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]), - hsv_image_mask); - } - else - { - cv::Scalar lower_color_range_0 = cv::Scalar(0, hsv_lower_bound_[1], hsv_lower_bound_[2]); - cv::Scalar upper_color_range_0 = cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]); - cv::Scalar lower_color_range_360 = cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]); - cv::Scalar upper_color_range_360 = cv::Scalar(180, hsv_upper_bound_[1], hsv_upper_bound_[2]); - cv::Mat output_image_0, output_image_360; - cv::inRange(hsv_image, lower_color_range_0, upper_color_range_0, output_image_0); - cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360); - hsv_image_mask = output_image_0 | output_image_360; - } - - /* find countour */ - std::vector > contours; - std::vector hierarchy; - cv::findContours(hsv_image_mask, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cv::Point(0,0)); - - if(contours.size() == 0) return false; - - /* find the biggest contour */ - float max_contour_area = 0; - float max_contour = 0; - for(int i = 0; i < contours.size(); ++i) - { - if (cv::contourArea(contours[i]) > max_contour_area) - { - max_contour_area = cv::contourArea(contours[i]); - max_contour = i; - } - } - cv::Moments contour_moments = cv::moments(contours[max_contour], true); - - /* publish the base information */ - geometry_msgs::PointStamped contour_center; - contour_center.header = image_msg->header; - contour_center.point.x = contour_moments.m10 / contour_moments.m00; - contour_center.point.y = contour_moments.m01 / contour_moments.m00; - pub_target_image_center_.publish(contour_center); - /* draw */ - cv::circle(src_image, cv::Point(contour_center.point.x, contour_center.point.y), 3, cv::Scalar(255, 0, 0), 5); - cv::drawContours(src_image, contours, max_contour, contour_color_, 3); - - /* publish */ - pub_target_image_.publish(cv_bridge::CvImage(image_msg->header, - image_msg->encoding, - src_image).toImageMsg()); - - - /* check whether the contour is satisfied the condistion */ - - /* we calculate the angle between the x axis(forward) and the line to the color region */ - float color_region_direction = atan2(-contour_center.point.x + camera_cx_, camera_fx_); - if(verbose_) cout << "vision detection direction" << color_region_direction << endl; - - /* Find the most closed tree to the color region, record the target tree index */ - float fov = atan2(camera_cx_, camera_fx_); //proximate the width of image with camera_cx_ - float min_diff = 1e6; - - vector cluster_index; - laserClustering(*scan_msg, cluster_index); - - for ( vector::iterator it = cluster_index.begin(); it != cluster_index.end(); ++it) - { - float diff = 0; - float laser_direction = *it * scan_msg->angle_increment + scan_msg->angle_min; + vision_detection::DetectorBase::initialize(nh, pnh); + pnh_.param("color_region_tree_clustering_angle_diff_thre", color_region_tree_clustering_angle_diff_thre_, 0.10); + + pnh_.param("h_min", hsv_lower_bound_[0], 0); + pnh_.param("s_min", hsv_lower_bound_[1], 0); + pnh_.param("v_min", hsv_lower_bound_[2], 0); + pnh_.param("h_max", hsv_upper_bound_[0], 255); + pnh_.param("s_max", hsv_upper_bound_[1], 255); + pnh_.param("v_max", hsv_upper_bound_[2], 255); + + pnh_.param("contour_color_r", contour_color_r_, 255); + pnh_.param("contour_color_g", contour_color_g_, 255); + pnh_.param("contour_color_b", contour_color_b_, 255); + contour_color_ = cv::Scalar(contour_color_r_, contour_color_g_, contour_color_b_); + + + /* ros service */ + srv_ = boost::make_shared > (nh_); + dynamic_reconfigure::Server::CallbackType f = + boost::bind (&IndependentHsvFilter::configCallback, this, _1, _2); + srv_->setCallback (f); + } + + void IndependentHsvFilter::configCallback(Config &new_config, uint32_t level) + { + if(!new_config.enable) return; + + ROS_WARN("New config"); + + hsv_lower_bound_[0] = new_config.color_hsv_h_min; + hsv_upper_bound_[0] = new_config.color_hsv_h_max; + hsv_lower_bound_[1] = new_config.color_hsv_s_min; + hsv_upper_bound_[1] = new_config.color_hsv_s_max; + hsv_lower_bound_[2] = new_config.color_hsv_v_min; + hsv_upper_bound_[2] = new_config.color_hsv_v_max; + } + + void IndependentHsvFilter::hsvFilter(cv::Mat src_image, string encoding, cv::Mat& hsv_image_mask) + { + cv::Mat hsv_image; + if(encoding == sensor_msgs::image_encodings::RGB8) + cv::cvtColor(src_image, hsv_image, CV_RGB2HSV); + else if(encoding == sensor_msgs::image_encodings::BGR8) + cv::cvtColor(src_image, hsv_image, CV_BGR2HSV); + + if(hsv_lower_bound_[0] < hsv_upper_bound_[0]) + { + cv::inRange(hsv_image, + cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]), + cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]), + hsv_image_mask); + } + else + { + cv::Scalar lower_color_range_0 = cv::Scalar(0, hsv_lower_bound_[1], hsv_lower_bound_[2]); + cv::Scalar upper_color_range_0 = cv::Scalar(hsv_upper_bound_[0], hsv_upper_bound_[1], hsv_upper_bound_[2]); + cv::Scalar lower_color_range_360 = cv::Scalar(hsv_lower_bound_[0], hsv_lower_bound_[1], hsv_lower_bound_[2]); + cv::Scalar upper_color_range_360 = cv::Scalar(180, hsv_upper_bound_[1], hsv_upper_bound_[2]); + cv::Mat output_image_0, output_image_360; + cv::inRange(hsv_image, lower_color_range_0, upper_color_range_0, output_image_0); + cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360); + hsv_image_mask = output_image_0 | output_image_360; + } + } + + bool IndependentHsvFilter::filter(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::LaserScanConstPtr& scan_msg, int& target_tree_index) + { + cv::Mat src_image = cv_bridge::toCvCopy(image_msg, image_msg->encoding)->image; + + /* general hsv filter */ + cv::Mat hsv_image_mask; + hsvFilter(src_image, image_msg->encoding, hsv_image_mask); + + /* find countour */ + std::vector > contours; + std::vector hierarchy; + cv::findContours(hsv_image_mask, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cv::Point(0,0)); + + if(contours.size() == 0) return false; + + /* find the biggest contour */ + float max_contour_area = 0; + float max_contour = 0; + for(int i = 0; i < contours.size(); ++i) + { + if (cv::contourArea(contours[i]) > max_contour_area) + { + max_contour_area = cv::contourArea(contours[i]); + max_contour = i; + } + } + cv::Moments contour_moments = cv::moments(contours[max_contour], true); + + /* publish the base information */ + geometry_msgs::PointStamped contour_center; + contour_center.header = image_msg->header; + contour_center.point.x = contour_moments.m10 / contour_moments.m00; + contour_center.point.y = contour_moments.m01 / contour_moments.m00; + pub_target_image_center_.publish(contour_center); + /* draw */ + cv::circle(src_image, cv::Point(contour_center.point.x, contour_center.point.y), 3, cv::Scalar(255, 0, 0), 5); + cv::drawContours(src_image, contours, max_contour, contour_color_, 3); + + /* publish */ + pub_target_image_.publish(cv_bridge::CvImage(image_msg->header, + image_msg->encoding, + src_image).toImageMsg()); + + + /* check whether the contour is satisfied the condistion */ + + /* we calculate the angle between the x axis(forward) and the line to the color region */ + float color_region_direction = atan2(-contour_center.point.x + camera_cx_, camera_fx_); + if(verbose_) cout << "vision detection direction" << color_region_direction << endl; + + /* Find the most closed tree to the color region, record the target tree index */ + float fov = atan2(camera_cx_, camera_fx_); //proximate the width of image with camera_cx_ + float min_diff = 1e6; + + vector cluster_index; + laserClustering(*scan_msg, cluster_index, src_image); + + for ( vector::iterator it = cluster_index.begin(); it != cluster_index.end(); ++it) + { + float diff = 0; + float laser_direction = *it * scan_msg->angle_increment + scan_msg->angle_min; + + /* if(fabs(laser_direction) > fov) - { - if(verbose_) cout << "eliminate: laser_direction:" << laser_direction << "; " << "fov: " << fov << endl; - continue; - } + { + + if(verbose_) cout << "eliminate: laser_direction:" << laser_direction << "; " << "fov: " << fov << endl; + continue; + } if(scan_msg->ranges[*it] > first_detection_depth_thre_) - { - if(verbose_) cout << "eliminate: depth: " << scan_msg->ranges[*it] << endl; - continue; - } - - diff = fabs(laser_direction - color_region_direction); - - if(verbose_) - { - cout << "tree index: " << *it << endl; - cout << "tree angle: " << laser_direction << endl; - cout << "diff: " << diff << endl; - } - - if(diff < min_diff) - { - min_diff = diff; - target_tree_index = *it; - } - - } - - /* Check whether the tree and color region is closed enough */ - if(min_diff > color_region_tree_clustering_angle_diff_thre_) - { - ROS_INFO_THROTTLE(1,"closest tree is far from the color region, can not find the target tree, diff: %f", min_diff); - return false; - } - - /* find the target tree */ - ROS_WARN("vision detectoion: find the target tree, angle_diff: %f", min_diff); - return true; - } - }; + { + if(verbose_) cout << "eliminate: depth: " << scan_msg->ranges[*it] << endl; + continue; + } + */ + if(!commonFiltering(laser_direction, fov, scan_msg->ranges[*it])) continue; + + diff = fabs(laser_direction - color_region_direction); + + if(verbose_) + { + cout << "tree index: " << *it << endl; + cout << "tree angle: " << laser_direction << endl; + cout << "diff: " << diff << endl; + } + + if(diff < min_diff) + { + min_diff = diff; + target_tree_index = *it; + } + + } + + /* Check whether the tree and color region is closed enough */ + if(min_diff > color_region_tree_clustering_angle_diff_thre_) + { + ROS_INFO_THROTTLE(1,"closest tree is far from the color region, can not find the target tree, diff: %f", min_diff); + return false; + } + + /* find the target tree */ + ROS_WARN("independent hsv filter based vision detectoion: find the target tree, angle_diff: %f", min_diff); + return true; + } }; - /* plugin registration */ #include PLUGINLIB_EXPORT_CLASS(vision_detection::IndependentHsvFilter, vision_detection::DetectorBase); diff --git a/jsk_uav_forest_perception/vision_detection_plugins.xml b/jsk_uav_forest_perception/vision_detection_plugins.xml index b306398..cc81d4b 100644 --- a/jsk_uav_forest_perception/vision_detection_plugins.xml +++ b/jsk_uav_forest_perception/vision_detection_plugins.xml @@ -4,4 +4,10 @@ Independent HSV filter. + + + Conditional HSV filter. + + + From 1a01a77c7bbb62cff0fa0376f39de443e31d8773 Mon Sep 17 00:00:00 2001 From: tongtybj Date: Mon, 24 Apr 2017 01:26:38 +0900 Subject: [PATCH 07/10] Add comment for the max radius thresh in tree clustering --- jsk_uav_forest_perception/param/LaserClustering.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_uav_forest_perception/param/LaserClustering.yaml b/jsk_uav_forest_perception/param/LaserClustering.yaml index a746b8a..02286d0 100644 --- a/jsk_uav_forest_perception/param/LaserClustering.yaml +++ b/jsk_uav_forest_perception/param/LaserClustering.yaml @@ -2,6 +2,6 @@ scan_filter_chain: - name: tree type: laser_filters/LaserClusteringFilter params: - max_radius: 0.4 + max_radius: 0.4 #kashiwa #0.25(diameter: 0.5) real min_points: 3 verbose: false From 41100bcdae53954dd0d84a2fdc264a09a3c9a28d Mon Sep 17 00:00:00 2001 From: tongtybj Date: Mon, 24 Apr 2017 01:31:01 +0900 Subject: [PATCH 08/10] Add the camera offset in X axis(image frame) to compenstate the physical offset between the center of hokuyo and camera --- .../jsk_uav_forest_perception/vision_detector_base_plugin.h | 4 +++- .../src/vision_detector/conditional_hsv_filter.cpp | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h index b9878db..428f82e 100644 --- a/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h +++ b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h @@ -101,6 +101,7 @@ namespace vision_detection pnh_.param("detect_once", detect_once_, true); pnh_.param("verbose", verbose_, true); + pnh_.param("camera_offset_x", camera_offset_x_, 0.0); pnh_.param("first_detection_depth_thre", first_detection_depth_thre_, 9.0); // [m] /* ros pub and sub */ @@ -145,6 +146,7 @@ namespace vision_detection bool verbose_; double first_detection_depth_thre_; + double camera_offset_x_; /* base var */ float scan_angle_increment_, scan_angle_min_; @@ -219,7 +221,7 @@ namespace vision_detection cluster_index.push_back(i); float laser_direction = i * scan_in.angle_increment + scan_in.angle_min; - cv::circle(src_image, cv::Point(camera_cx_ - camera_fx_ * tan(laser_direction) , camera_cy_), 3, cv::Scalar(255, 0, 0), 5); + cv::circle(src_image, cv::Point(camera_cx_ + camera_offset_x_ - camera_fx_ * tan(laser_direction) , camera_cy_), 3, cv::Scalar(255, 0, 0), 5); } } } diff --git a/jsk_uav_forest_perception/src/vision_detector/conditional_hsv_filter.cpp b/jsk_uav_forest_perception/src/vision_detector/conditional_hsv_filter.cpp index a8da0ac..84b7adc 100644 --- a/jsk_uav_forest_perception/src/vision_detector/conditional_hsv_filter.cpp +++ b/jsk_uav_forest_perception/src/vision_detector/conditional_hsv_filter.cpp @@ -79,8 +79,8 @@ namespace vision_detection float y1 = laser_distance * sin(laser_direction) + tree_diameter_ / 2 * sin(laser_direction - M_PI/2); float y2 = laser_distance * sin(laser_direction) + tree_diameter_ / 2 * sin(laser_direction + M_PI/2); - float x_l = camera_cx_ - camera_fx_ * y2 / x2; - float x_r = camera_cx_ - camera_fx_ * y1 / x1; + float x_l = camera_cx_ + camera_offset_x_ - camera_fx_ * y2 / x2; + float x_r = camera_cx_ + camera_offset_x_ - camera_fx_ * y1 / x1; if(x_l < 0) x_l = 0; if(x_r > src_image.cols) x_r = src_image.cols; if(verbose_) From 716480967907ca6a93b4b39e2987c71d7d163559 Mon Sep 17 00:00:00 2001 From: tongtybj Date: Mon, 24 Apr 2017 01:54:32 +0900 Subject: [PATCH 09/10] Add the target tree marking in independent hsv filter --- .../independent_hsv_filter.cpp | 36 +++++++------------ 1 file changed, 12 insertions(+), 24 deletions(-) diff --git a/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp b/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp index dc36275..fc64133 100644 --- a/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp +++ b/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp @@ -143,16 +143,13 @@ namespace vision_detection contour_center.point.y = contour_moments.m01 / contour_moments.m00; pub_target_image_center_.publish(contour_center); /* draw */ - cv::circle(src_image, cv::Point(contour_center.point.x, contour_center.point.y), 3, cv::Scalar(255, 0, 0), 5); + cv::circle(src_image, cv::Point(contour_center.point.x, contour_center.point.y), 3, cv::Scalar(0, 0, 255), 5); cv::drawContours(src_image, contours, max_contour, contour_color_, 3); - /* publish */ - pub_target_image_.publish(cv_bridge::CvImage(image_msg->header, - image_msg->encoding, - src_image).toImageMsg()); - - /* check whether the contour is satisfied the condistion */ + /* laser preparation */ + vector cluster_index; + laserClustering(*scan_msg, cluster_index, src_image); /* we calculate the angle between the x axis(forward) and the line to the color region */ float color_region_direction = atan2(-contour_center.point.x + camera_cx_, camera_fx_); @@ -162,28 +159,11 @@ namespace vision_detection float fov = atan2(camera_cx_, camera_fx_); //proximate the width of image with camera_cx_ float min_diff = 1e6; - vector cluster_index; - laserClustering(*scan_msg, cluster_index, src_image); - for ( vector::iterator it = cluster_index.begin(); it != cluster_index.end(); ++it) { float diff = 0; float laser_direction = *it * scan_msg->angle_increment + scan_msg->angle_min; - /* - if(fabs(laser_direction) > fov) - { - - if(verbose_) cout << "eliminate: laser_direction:" << laser_direction << "; " << "fov: " << fov << endl; - continue; - } - - if(scan_msg->ranges[*it] > first_detection_depth_thre_) - { - if(verbose_) cout << "eliminate: depth: " << scan_msg->ranges[*it] << endl; - continue; - } - */ if(!commonFiltering(laser_direction, fov, scan_msg->ranges[*it])) continue; diff = fabs(laser_direction - color_region_direction); @@ -210,6 +190,14 @@ namespace vision_detection return false; } + /* draw */ + cv::circle(src_image, cv::Point(camera_cx_ + camera_offset_x_ - camera_fx_ * tan(target_tree_index * scan_msg->angle_increment + scan_msg->angle_min) , camera_cy_), 3, cv::Scalar(255, 0, 0), 20); + + /* publish */ + pub_target_image_.publish(cv_bridge::CvImage(image_msg->header, + image_msg->encoding, + src_image).toImageMsg()); + /* find the target tree */ ROS_WARN("independent hsv filter based vision detectoion: find the target tree, angle_diff: %f", min_diff); return true; From 46c3963a8dc732c7543709332154aa06cc63bc28 Mon Sep 17 00:00:00 2001 From: tongtybj Date: Mon, 24 Apr 2017 01:55:17 +0900 Subject: [PATCH 10/10] Add two parameter for vision detection in image_process.launch --- jsk_uav_forest_perception/launch/image_process.launch | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_uav_forest_perception/launch/image_process.launch b/jsk_uav_forest_perception/launch/image_process.launch index a773449..0f9e06a 100644 --- a/jsk_uav_forest_perception/launch/image_process.launch +++ b/jsk_uav_forest_perception/launch/image_process.launch @@ -12,6 +12,8 @@ + +