Merge pull request #32 from tongtybj/perception

[Target Tree Detection and Tracking]Enhace the vision detection

jsk_uav_forest_common/launch/debug.launch

@@ -2,12 +2,20 @@
 <launch>
   <arg name="use_dji" default="True" />
   <arg name="headless" default="False" />
-
+  <arg name="no_odom" default="False" />
+
   ############## Viewer ###############
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find jsk_uav_forest_common)/launch/log_viewer.rviz" unless="$(arg headless)" respawn="true"/>
 
   ############## Odometry ###############
-  <node pkg="jsk_uav_forest_common" type="odom_generator.py" name="odom_gen" output="screen" />
+  <node pkg="jsk_uav_forest_common" type="odom_generator.py" name="odom_gen" output="screen" if="$(arg no_odom)">
+    <param name="use_lidar" value="True" />
+    <param name="use_guidance_vel" value="True" />
+    <param name="lidar_tc" value="0.5" />
+    <param name="lidar_noise_cut_thresh" value="0.5" />
+    <param name="guidance_vel_weight" value="0.9" />
+  </node>
+
   <node pkg="jsk_uav_forest_common" type="tf_broadcaster.py" name="tf_broadcaster" output="screen" >
     <param name="uav_odom_topic_name" value="/modified_odom" if="$(arg use_dji)" />
     <param name="uav_odom_topic_name" value="/ground_truth/state" unless="$(arg use_dji)" />
@@ -17,7 +25,6 @@
   ######### Perception Node #########
     <!-- vision base -->
   <include file="$(find jsk_uav_forest_perception)/launch/image_process.launch">
-    <arg name="independent_nodelet" value="true" />
     <arg name="image_name" value="/camera/image_rect"/>
   </include>
 
@@ -26,11 +33,8 @@
   </include>
 
   <!-- tree detector -->
-  <node pkg="jsk_uav_forest_perception" type="tree_detector_node" output="screen" name="tree_detector_node">
-    <param name="color_region_center_topic_name" value="/object_image_center" />
-    <param name="camera_info_topic_name" value="/camera/camera_info" />
-    <param name="laser_scan_topic_name" value="/shadow_filtered" />
-    <param name="clurstering_max_radius" value="0.4" />
+  <node pkg="jsk_uav_forest_perception" type="tree_tracking_node" output="screen" name="tree_tracking_node">
+     <param name="laser_scan_topic_name" value="/scan_clustered" />
     <param name="uav_odom_topic_name" value="/modified_odom"/>
     <param name="verbose" value="false" />
   </node>

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)

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)
 
 
 #############

jsk_uav_forest_perception/cfg/ColorRegionDetector.cfg → 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"))

jsk_uav_forest_perception/include/jsk_uav_forest_perception/laser_scan_filters.h → jsk_uav_forest_perception/include/jsk_uav_forest_perception/laser_clustering_filter.h

@@ -38,21 +38,23 @@
 #include <sensor_msgs/LaserScan.h>
 #include <angles/angles.h>
 
+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<sensor_msgs::LaserScan>
+class LaserClusteringFilter : public filters::FilterBase<sensor_msgs::LaserScan>
 {
 public:
 
   double max_radius_;          // Filter angle threshold
   int min_points_;
-    
+  bool verbose_;
 
   ////////////////////////////////////////////////////////////////////////////////
-  ScanTreeFilter () 
+  LaserClusteringFilter ()
   {
 
 
@@ -74,11 +76,16 @@ class ScanTreeFilter : public filters::FilterBase<sensor_msgs::LaserScan>
       ROS_INFO("Error: TreeFilter was not given min_points.\n");
       return false;
     }
+    if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("verbose"), verbose_))
+    {
+      verbose_ = false;
+    }
+
     return true;
   }
 
   ////////////////////////////////////////////////////////////////////////////////
-  virtual ~ScanTreeFilter () { }
+  virtual ~LaserClusteringFilter () { }
 
   ////////////////////////////////////////////////////////////////////////////////
   /** \brief 
@@ -95,49 +102,70 @@ class ScanTreeFilter : public filters::FilterBase<sensor_msgs::LaserScan>
     std::vector<std::vector<int> > range_blobs;
     std::vector<int> 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<int>::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<sensor_msgs::LaserScan>
 } ;
 }
 
-#endif //LASER_SCAN_TREE_FILTER_H
+#endif //LASER_CUSTERING_FILTER_H

jsk_uav_forest_perception/include/jsk_uav_forest_perception/tree_detector.h → jsk_uav_forest_perception/include/jsk_uav_forest_perception/tree_tracking.h

@@ -33,66 +33,57 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-#ifndef TREE_DETECTOR_H_
-#define TREE_DETECTOR_H_
+#ifndef TREE_TRACKING_H_
+#define TREE_TRACKING_H_
 
 /* ros */
 #include <ros/ros.h>
+
+/* message filter */
 #include <message_filters/subscriber.h>
 #include <message_filters/time_synchronizer.h>
 
 /* ros msg/srv */
 #include <sensor_msgs/CameraInfo.h>
-#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/Vector3Stamped.h>
 #include <sensor_msgs/LaserScan.h>
 #include <nav_msgs/Odometry.h>
 #include <std_msgs/Bool.h>
 #include <tf/transform_broadcaster.h>
 
 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<sensor_msgs::LaserScan, geometry_msgs::Vector3Stamped> SyncPolicy;
 private:
   ros::NodeHandle nh_, nhp_;
+
+  boost::shared_ptr<SyncPolicy> sync_;
+  message_filters::Subscriber<geometry_msgs::Vector3Stamped> sub_sync_vision_detection_;
+  message_filters::Subscriber<sensor_msgs::LaserScan> 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<int>& 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