diff --git a/jsk_uav_forest_common/launch/debug.launch b/jsk_uav_forest_common/launch/debug.launch index 4db5835..ed56778 100644 --- a/jsk_uav_forest_common/launch/debug.launch +++ b/jsk_uav_forest_common/launch/debug.launch @@ -2,12 +2,20 @@ - + + ############## Viewer ############### ############## Odometry ############### - + + + + + + + + @@ -17,7 +25,6 @@ ######### Perception Node ######### - @@ -26,11 +33,8 @@ - - - - - + + 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) diff --git a/jsk_uav_forest_perception/CMakeLists.txt b/jsk_uav_forest_perception/CMakeLists.txt index 026d650..9eea249 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,22 @@ 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_pluginlib + 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) -add_library(jsk_laser_scan_filters SHARED src/laser_scan_filters.cpp) -target_link_libraries(jsk_laser_scan_filters ${catkin_LIBRARIES}) +add_executable(vision_detection_node src/vision_detection.cpp) +target_link_libraries (vision_detection_node ${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/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/color_region_detector.h b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector/independent_hsv_filter.h similarity index 60% 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/independent_hsv_filter.h index 99eea19..8715a92 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/independent_hsv_filter.h @@ -2,7 +2,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2016, JSK Lab + * Copyright (c) 2017, JSK Lab * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,65 +33,33 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +/* base class */ +#include -#ifndef COLOR_REGION_DETECTOR_H_ -#define COLOR_REGION_DETECTOR_H_ +using namespace std; -/* ros */ -#include -#include - -/* cfg */ -#include -#include - -/* ros msg */ -#include -#include -#include - -/* cv */ -#include -#include -#include - -/* standard */ -#include -#include - -namespace color_region_detection +namespace vision_detection { - class ColorRegionDetector: public nodelet::Nodelet + typedef jsk_uav_forest_perception::HsvFilterConfig Config; + class IndependentHsvFilter :public vision_detection::DetectorBase { public: - ColorRegionDetector(){} - ~ColorRegionDetector(); - typedef jsk_uav_forest_perception::ColorRegionDetectorConfig Config; - - private: - ros::NodeHandle pnh_; - ros::Subscriber sub_camera_image_; - ros::Subscriber sub_camera_info_; - ros::Publisher pub_target_image_; - ros::Publisher pub_target_image_center_; + virtual void initialize(ros::NodeHandle nh, ros::NodeHandle pnh); + protected: boost::shared_ptr > srv_; - std::string image_topic_name_; - std::string out_image_topic_name_; - std::string camera_info_topic_name_; - + 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_; - 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); - virtual void onInit(); - void cameraDataCallback(const sensor_msgs::ImageConstPtr& image_msg); - void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info); + + 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); }; -} -#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..428f82e --- /dev/null +++ b/jsk_uav_forest_perception/include/jsk_uav_forest_perception/vision_detector_base_plugin.h @@ -0,0 +1,249 @@ +// -*- 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("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 */ + 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_; + double camera_offset_x_; + + /* 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, 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); + + float laser_direction = i * scan_in.angle_increment + scan_in.angle_min; + 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); + } + } + } + + 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; + }; +} + +#endif 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..0f9e06a 100644 --- a/jsk_uav_forest_perception/launch/image_process.launch +++ b/jsk_uav_forest_perception/launch/image_process.launch @@ -1,22 +1,21 @@ - - + - - - - - - - - - - - - + + + + + + + + + + + + diff --git a/jsk_uav_forest_perception/launch/laser_process.launch b/jsk_uav_forest_perception/launch/laser_process.launch index 857bbbc..473eb68 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..aa4b618 100644 --- a/jsk_uav_forest_perception/launch/tree_detector.launch +++ b/jsk_uav_forest_perception/launch/tree_detector.launch @@ -3,10 +3,9 @@ - + - @@ -15,15 +14,10 @@ - - - - - - + + - - + diff --git a/jsk_uav_forest_perception/package.xml b/jsk_uav_forest_perception/package.xml index 393c60d..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/ColorRegionDetection.yaml b/jsk_uav_forest_perception/param/ColorRegionDetection.yaml deleted file mode 100644 index fc5a57f..0000000 --- a/jsk_uav_forest_perception/param/ColorRegionDetection.yaml +++ /dev/null @@ -1,8 +0,0 @@ -camera: - color_region_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/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/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/LaserClustering.yaml b/jsk_uav_forest_perception/param/LaserClustering.yaml new file mode 100644 index 0000000..02286d0 --- /dev/null +++ b/jsk_uav_forest_perception/param/LaserClustering.yaml @@ -0,0 +1,7 @@ +scan_filter_chain: +- name: tree + type: laser_filters/LaserClusteringFilter + params: + max_radius: 0.4 #kashiwa #0.25(diameter: 0.5) real + min_points: 3 + verbose: false 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 deleted file mode 100644 index 56240bc..0000000 --- a/jsk_uav_forest_perception/param/TreeFilter.yaml +++ /dev/null @@ -1,7 +0,0 @@ -scan_filter_chain: -- name: tree - type: laser_filters/ScanTreeFilter - params: - max_radius: 0.5 - min_points: 3 - 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..ed50b0b --- /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_, 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_, 100)); + 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_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/conditional_hsv_filter.cpp b/jsk_uav_forest_perception/src/vision_detector/conditional_hsv_filter.cpp new file mode 100644 index 0000000..84b7adc --- /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_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_) + 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/color_region_detector.cpp b/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp similarity index 50% rename from jsk_uav_forest_perception/src/color_region_detector.cpp rename to jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp index 088aff6..fc64133 100644 --- a/jsk_uav_forest_perception/src/color_region_detector.cpp +++ b/jsk_uav_forest_perception/src/vision_detector/independent_hsv_filter.cpp @@ -33,24 +33,18 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +/* base class */ +#include -#include "jsk_uav_forest_perception/color_region_detector.h" +using namespace std; -namespace color_region_detection +namespace vision_detection { - ColorRegionDetector::~ColorRegionDetector() + void IndependentHsvFilter::initialize(ros::NodeHandle nh, ros::NodeHandle pnh) { + vision_detection::DetectorBase::initialize(nh, pnh); - } - - 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("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); @@ -64,21 +58,15 @@ namespace color_region_detection 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_); + srv_ = boost::make_shared > (nh_); dynamic_reconfigure::Server::CallbackType f = - boost::bind (&ColorRegionDetector::configCallback, this, _1, _2); + boost::bind (&IndependentHsvFilter::configCallback, this, _1, _2); srv_->setCallback (f); } - void ColorRegionDetector::configCallback(Config &new_config, uint32_t level) + void IndependentHsvFilter::configCallback(Config &new_config, uint32_t level) { if(!new_config.enable) return; @@ -92,32 +80,13 @@ namespace color_region_detection 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) + void IndependentHsvFilter::hsvFilter(cv::Mat src_image, string encoding, cv::Mat& hsv_image_mask) { - 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); - } + 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]) { @@ -137,44 +106,106 @@ namespace color_region_detection 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) + + 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) { - 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) { - if (cv::contourArea(contours[i]) > max_contour_area) - { - max_contour_area = cv::contourArea(contours[i]); - max_contour = i; - } + 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()); + 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(0, 0, 255), 5); + cv::drawContours(src_image, contours, max_contour, contour_color_, 3); + + /* 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_); + 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; + + 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(!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; + } + + /* 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; } -} +}; + +/* plugin registration */ #include -PLUGINLIB_EXPORT_CLASS (color_region_detection::ColorRegionDetector, nodelet::Nodelet); +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 new file mode 100644 index 0000000..cc81d4b --- /dev/null +++ b/jsk_uav_forest_perception/vision_detection_plugins.xml @@ -0,0 +1,13 @@ + + + + Independent HSV filter. + + + + + Conditional HSV filter. + + + + 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 @@ - - + +