diff --git a/tree_tracking/CMakeLists.txt b/tree_tracking/CMakeLists.txt new file mode 100644 index 000000000..986e76ae1 --- /dev/null +++ b/tree_tracking/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 3.0.2) +project(tree_tracking) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + cmake_modules + cv_bridge + dynamic_reconfigure + geometry_msgs + image_geometry + image_transport + nodelet + roscpp + sensor_msgs + std_msgs + tf + laser_filters + filters +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +add_compile_options(-std=c++11) + +find_package(OpenCV REQUIRED) +message(WARNING "OPENCV ${OpenCV_VERSION} FOUND") + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp sensor_msgs nodelet tf std_msgs dynamic_reconfigure geometry_msgs + LIBRARIES tree_tracking +) + +########### +## Build ## +########### +include_directories( + include + ${OpenCV_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ) + +# catkin_package( +# INCLUDE_DIRS include +# CATKIN_DEPENDS roscpp sensor_msgs nodelet tf std_msgs dynamic_reconfigure geometry_msgs +# # LIBRARIES tree_tracking +# ) + +add_library(laser_clustering_filter SHARED src/laser_clustering_filter.cpp) +target_link_libraries(laser_clustering_filter ${catkin_LIBRARIES}) + +add_library(tree_tracking src/tree_tracking.cpp src/tree_database.cpp src/circle_detection.cpp) +target_link_libraries(tree_tracking ${catkin_LIBRARIES}) + +install(TARGETS tree_tracking + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +add_executable(tree_tracking_node src/tree_tracking_node.cpp) +target_link_libraries (tree_tracking_node ${catkin_LIBRARIES} tree_tracking) + +############# +## Install ## +############# +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install( + TARGETS tree_tracking_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) +install( + FILES laser_filters_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/tree_tracking/config/rviz_config.rviz b/tree_tracking/config/rviz_config.rviz new file mode 100644 index 000000000..0c9e059fa --- /dev/null +++ b/tree_tracking/config/rviz_config.rviz @@ -0,0 +1,227 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /LaserScan1 + - /LaserScan2 + - /LaserScan3 + - /MarkerArray1 + - /Axes1 + Splitter Ratio: 0.45338207483291626 + Tree Height: 1239 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /multirotor/scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /multirotor/shadow_filtered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /multirotor/scan_clustered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /multirotor/visualization_marker + Name: MarkerArray + Namespaces: + tree: true + tree_diameter: true + Queue Size: 100 + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: multirotor/main_body + Show Trail: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 12.02877140045166 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 1.8418309688568115 + Y: 0.056299153715372086 + Z: -0.24842405319213867 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.14479660987854 + Target Frame: + Yaw: 3.213582992553711 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1536 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000021900000562fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000562000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000562fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000562000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003f10000003efc0100000002fb0000000800540069006d00650100000000000003f1000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003f10000056200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1009 + X: 1551 + Y: 27 diff --git a/tree_tracking/include/tree_tracking/circle_detection.h b/tree_tracking/include/tree_tracking/circle_detection.h new file mode 100644 index 000000000..f3aac855d --- /dev/null +++ b/tree_tracking/include/tree_tracking/circle_detection.h @@ -0,0 +1,10 @@ +#ifndef UTIL_CIRCLE_DETECTION_H +#define UTIL_CIRCLE_DETECTION_H + +#include +#include + +namespace CircleDetection { + void circleFitting(const std::vector& points, tf::Vector3& tree_center_location, double& tree_radius, double& regulation); +} +#endif diff --git a/tree_tracking/include/tree_tracking/laser_clustering_filter.h b/tree_tracking/include/tree_tracking/laser_clustering_filter.h new file mode 100644 index 000000000..9cca024f2 --- /dev/null +++ b/tree_tracking/include/tree_tracking/laser_clustering_filter.h @@ -0,0 +1,181 @@ +/* + * Copyright (c) 2008 Radu Bogdan Rusu + * + * 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/or other materials provided with the distribution. + * + * 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 LASER_SCAN_TREE_FILTER_H +#define LASER_SCAN_TREE_FILTER_H + +#include + +#include +#include +#include + +using namespace std; + +namespace laser_filters{ + +/** @b LaserClusteringFilter is a simple filter that filters shadow points in a laser scan line + */ + +class LaserClusteringFilter : public filters::FilterBase +{ +public: + + double max_radius_; // Filter angle threshold + int min_points_; + bool verbose_; + + //////////////////////////////////////////////////////////////////////////////// + LaserClusteringFilter () + { + + + } + + /**@b Configure the filter from XML */ + bool configure() + { + max_radius_ = 0.1;//default value + if (!filters::FilterBase::getParam(std::string("max_radius"), max_radius_)) + { + ROS_ERROR("Error: TreeFilter was not given min_radius.\n"); + return false; + } + + min_points_ = 5;//default value + if (!filters::FilterBase::getParam(std::string("min_points"), min_points_)) + { + 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 ~LaserClusteringFilter () { } + + //////////////////////////////////////////////////////////////////////////////// + /** \brief + * \param scan_in the input LaserScan message + * \param scan_out the output LaserScan message + */ + bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out) + { + //copy across all data first + scan_out = scan_in; + + 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() > min_points_ ) { + range_blobs.push_back(range_blob); + } + range_blob.clear(); + }else{ + range_blob.push_back(i); + } + } + if ( range_blob.size() > 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 < 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 (*) + } + return true; + } + + //////////////////////////////////////////////////////////////////////////////// + +} ; +} + +#endif //LASER_CUSTERING_FILTER_H diff --git a/tree_tracking/include/tree_tracking/tree_database.h b/tree_tracking/include/tree_tracking/tree_database.h new file mode 100644 index 000000000..9f6a57600 --- /dev/null +++ b/tree_tracking/include/tree_tracking/tree_database.h @@ -0,0 +1,93 @@ +#ifndef TREE_DATABASE_H_ +#define TREE_DATABASE_H_ + +/* ros */ +#include + +/* ros msg/srv */ +#include +#include +#include +#include + +/* uitls */ +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +class TreeHandle +{ +public: + TreeHandle(): pos_(0,0,0), vote_(0), radius_(0) {} + TreeHandle(ros::NodeHandle nh, ros::NodeHandle nhp, tf::Vector3 pos); + ~TreeHandle(){} + + boost::shared_ptr getHandle() { return boost::shared_ptr(this); } + void updatePos(const tf::Vector3& pos, bool lpf = true); + void setRadius(double radius, bool lpf = true); + const double getRadius(){ return radius_; } + const tf::Vector3 getPos() { return pos_; } + inline void setVote(int vote) {vote_ = vote; } + inline int getVote() { return vote_; } + +private: + ros::NodeHandle nh_, nhp_; + + double filter_rate_; + + tf::Vector3 pos_; + double radius_; + int vote_; +}; + +typedef boost::shared_ptr TreeHandlePtr; + +bool operator>(const TreeHandlePtr& left, const TreeHandlePtr& right); + +class TreeDataBase +{ +public: + TreeDataBase(ros::NodeHandle nh, ros::NodeHandle nhp); + ~TreeDataBase(){} + + void add(const TreeHandlePtr new_tree); + bool updateSingleTree(const tf::Vector3& tree_pos, const double& tree_radius, const bool only_target = false); + + void update(); + void setCenterTree(TreeHandlePtr center_tree) { center_tree_ = center_tree; } + void visualization(std_msgs::Header header); + void save(); + bool load(string file_name); + + int getIndex(TreeHandlePtr target_tree); + + int validTreeNum() + { + return (trees_.size()>valid_num_)?valid_num_:trees_.size(); + } + + inline void getTrees(vector& trees) { trees = trees_; } +private: + ros::NodeHandle nh_, nhp_; + + /* ros param */ + double tree_margin_radius_; /* the margin area to check the candidate position for a tree */ + int valid_num_; /* the number of valid tree to */ + bool verbose_; + bool visualization_; + double tree_cut_rate_; + string visualization_marker_topic_name_; + + ros::Publisher pub_visualization_marker_; + + /* the set for trees */ + vector trees_; + TreeHandlePtr center_tree_; +}; +#endif diff --git a/tree_tracking/include/tree_tracking/tree_tracking.h b/tree_tracking/include/tree_tracking/tree_tracking.h new file mode 100644 index 000000000..8fbe05dcb --- /dev/null +++ b/tree_tracking/include/tree_tracking/tree_tracking.h @@ -0,0 +1,55 @@ +#ifndef TREE_TRACKING_H_ +#define TREE_TRACKING_H_ + +/* ros */ +#include + +/* ros msg/srv */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* tree database */ +#include "tree_database.h" // + +using namespace std; + +class TreeTracking +{ +public: + TreeTracking(ros::NodeHandle nh, ros::NodeHandle nhp); + ~TreeTracking(){} + +private: + ros::NodeHandle nh_, nhp_; + + ros::Subscriber sub_laser_scan_; + ros::Subscriber sub_odom_; + ros::Publisher pub_visualization_marker_; + string laser_scan_topic_name_, odom_topic_name_; + string tree_global_location_topic_name_; + string visualization_marker_topic_name_; + + TreeDataBase tree_db_; + vector target_trees_; + + tf::Vector3 uav_odom_; + float uav_roll_, uav_pitch_, uav_yaw_; + double tree_scan_angle_thre_; + double tree_circle_regulation_thre_; + double tree_radius_max_, tree_radius_min_; + + double urg_yaw_offset_; + // void visionDetectionCallback(const geometry_msgs::Vector3StampedConstPtr& vision_detection_msg); + void uavOdomCallback(const nav_msgs::OdometryConstPtr& uav_msg); + void laserScanCallback(const sensor_msgs::LaserScanConstPtr& scan_msg); + +}; + +#endif diff --git a/tree_tracking/laser_filters_plugins.xml b/tree_tracking/laser_filters_plugins.xml new file mode 100644 index 000000000..f1aca41fb --- /dev/null +++ b/tree_tracking/laser_filters_plugins.xml @@ -0,0 +1,10 @@ + + + + + This is a filter which do clustering from a laser. + + + + diff --git a/tree_tracking/launch/bringup.launch b/tree_tracking/launch/bringup.launch new file mode 100644 index 000000000..974b62274 --- /dev/null +++ b/tree_tracking/launch/bringup.launch @@ -0,0 +1,21 @@ + + + ########### launch config ########### + + + + + + + + + + + + + + + + + + diff --git a/tree_tracking/launch/laser_process.launch b/tree_tracking/launch/laser_process.launch new file mode 100644 index 000000000..291929ad0 --- /dev/null +++ b/tree_tracking/launch/laser_process.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/tree_tracking/launch/tree_detector.launch b/tree_tracking/launch/tree_detector.launch new file mode 100644 index 000000000..bd30d4824 --- /dev/null +++ b/tree_tracking/launch/tree_detector.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/tree_tracking/launch/tree_detector_visualize.launch b/tree_tracking/launch/tree_detector_visualize.launch new file mode 100644 index 000000000..05474953f --- /dev/null +++ b/tree_tracking/launch/tree_detector_visualize.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/tree_tracking/launch/urg_node.launch b/tree_tracking/launch/urg_node.launch new file mode 100644 index 000000000..27f4c8d3c --- /dev/null +++ b/tree_tracking/launch/urg_node.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/tree_tracking/package.xml b/tree_tracking/package.xml new file mode 100644 index 000000000..7a1acb837 --- /dev/null +++ b/tree_tracking/package.xml @@ -0,0 +1,63 @@ + + + tree_tracking + 0.0.0 + The tree tracking package + + Haruki Kozuka + + BSD + + + catkin + cmake_modules + cv_bridge + dynamic_reconfigure + geometry_msgs + image_geometry + image_transport + nodelet + roscpp + sensor_msgs + std_msgs + tf + laser_filters + filters + + cmake_modules + cv_bridge + dynamic_reconfigure + geometry_msgs + image_geometry + image_transport + nodelet + roscpp + sensor_msgs + std_msgs + tf + laser_filters + filters + + cmake_modules + cv_bridge + dynamic_reconfigure + geometry_msgs + image_geometry + image_transport + nodelet + roscpp + sensor_msgs + std_msgs + tf + laser_filters + filters + rviz + urg_node + + + + + + + + diff --git a/tree_tracking/param/LaserClustering.yaml b/tree_tracking/param/LaserClustering.yaml new file mode 100644 index 000000000..02286d044 --- /dev/null +++ b/tree_tracking/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/tree_tracking/param/LaserShadowFilter.yaml b/tree_tracking/param/LaserShadowFilter.yaml new file mode 100644 index 000000000..a51e66422 --- /dev/null +++ b/tree_tracking/param/LaserShadowFilter.yaml @@ -0,0 +1,13 @@ +scan_filter_chain: +- name: shadows + type: laser_filters/ScanShadowsFilter + params: + min_angle: 10 + max_angle: 170 + neighbors: 3 + window: 3 +- name: range + type: laser_filters/LaserScanRangeFilter + params: + lower_threshold: 0.2 + upper_threshold: 15 diff --git a/tree_tracking/src/circle_detection.cpp b/tree_tracking/src/circle_detection.cpp new file mode 100644 index 000000000..c39399031 --- /dev/null +++ b/tree_tracking/src/circle_detection.cpp @@ -0,0 +1,32 @@ +#include "tree_tracking/circle_detection.h" + +namespace CircleDetection { + void circleFitting(const std::vector& points, tf::Vector3& tree_center_location, double& tree_radius, double& regulation) + { + double val[8] = {0,0,0,0,0,0,0,0}; + for (std::vector::const_iterator it = points.begin(); it != points.end(); it++) { + double x = it->getX(); double y = it->getY(); + val[0] += x; + val[1] += y; + val[2] += x * x; + val[3] += y * y; + val[4] += x * y; + val[5] += -(x * x * x + x * y * y); + val[6] += -(x * x * y + y * y * y); + } + val[7] = points.size(); + + tf::Matrix3x3 m(val[2], val[4], val[0], val[4], val[3], val[1], val[0], val[1], val[7]); + tf::Vector3 v(val[5], val[6], -val[2] - val[3]); + tf::Vector3 solution = m.inverse() * v; + tree_center_location.setValue(solution.x() * -0.5, solution.y() * -0.5, 0); + tree_radius = std::sqrt(solution.x() * solution.x() / 4 + solution.y() * solution.y() / 4 - solution.z()); + double variation = 0.0; + for (std::vector::const_iterator it = points.begin(); it != points.end(); it++) { + double dx = it->getX() - tree_center_location.x(); double dy = it->getY() - tree_center_location.y(); + double err = dx * dx + dy * dy - tree_radius * tree_radius; + variation += err * err; + } + regulation = variation / (points.size() * tree_radius * tree_radius * tree_radius * tree_radius); + } +} diff --git a/tree_tracking/src/laser_clustering_filter.cpp b/tree_tracking/src/laser_clustering_filter.cpp new file mode 100644 index 000000000..ab3d26401 --- /dev/null +++ b/tree_tracking/src/laser_clustering_filter.cpp @@ -0,0 +1,37 @@ +/* + * Copyright (c) 2017, JSK. + * 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/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. 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 "tree_tracking/laser_clustering_filter.h" +#include "sensor_msgs/LaserScan.h" +#include "filters/filter_base.h" + +#include "pluginlib/class_list_macros.h" + + +PLUGINLIB_EXPORT_CLASS(laser_filters::LaserClusteringFilter, filters::FilterBase) diff --git a/tree_tracking/src/tree_database.cpp b/tree_tracking/src/tree_database.cpp new file mode 100644 index 000000000..044458682 --- /dev/null +++ b/tree_tracking/src/tree_database.cpp @@ -0,0 +1,249 @@ +#include "tree_tracking/tree_database.h" + +TreeHandle::TreeHandle(ros::NodeHandle nh, ros::NodeHandle nhp, tf::Vector3 pos):nh_(nh), nhp_(nhp), pos_(pos), vote_(1), radius_(-1) +{ + nhp_.param("filter_rate", filter_rate_, 0.8); +} + +void TreeHandle::updatePos(const tf::Vector3& pos, bool lpf) +{ + if(lpf) pos_ = filter_rate_ * pos_ + (1 - filter_rate_) * pos; + else pos_ = pos; + vote_++; +} + +void TreeHandle::setRadius(double radius, bool lpf) +{ + if (radius_ < 0) { + radius_ = radius; //init + } else { + if(lpf) radius_ = filter_rate_ * radius_ + (1 - filter_rate_) * radius; + else radius_ = radius; + } +} + +bool operator>(const TreeHandlePtr& left, const TreeHandlePtr& right) +{ + return left->getVote() > right->getVote() ; +} + +TreeDataBase::TreeDataBase(ros::NodeHandle nh, ros::NodeHandle nhp):nh_(nh), nhp_(nhp) +{ + trees_.resize(0); + nhp_.param("tree_margin_radius", tree_margin_radius_, 1.0); // 1.0[m] + nhp_.param("valid_num", valid_num_, 7); + nhp_.param("verbose", verbose_, false); + nhp_.param("visualization_marker_topic_name", visualization_marker_topic_name_, string("visualization_marker")); + nhp_.param("tree_cut_rate", tree_cut_rate_, 0.1); + pub_visualization_marker_ = nh_.advertise(visualization_marker_topic_name_, 1); +} + +void TreeDataBase::add(const TreeHandlePtr new_tree) +{ + trees_.push_back(new_tree); + ROS_INFO("add new tree No.%d: [%f, %f]", (int)trees_.size(), new_tree->getPos().x(), new_tree->getPos().y()); +} + +bool TreeDataBase::updateSingleTree(const tf::Vector3& tree_pos, const double& tree_radius, const bool only_target) +{ + bool new_tree = true; + float min_dist = 1e6; + TreeHandlePtr target_tree; + int tree_index = 0; + for(vector::iterator it = trees_.begin(); it != trees_.end(); ++it) + { + float dist = (tree_pos - (*it)->getPos()).length(); + size_t index = distance(trees_.begin(), it); + + /* we assume that the distance of any two trees is more than min_ditance_ */ + if(dist < tree_margin_radius_) + { + if(!new_tree) + { + if(verbose_) ROS_WARN("there are two trees which are to close to each other"); + } + new_tree = false; + } + + /* update */ + if(min_dist > dist) + { + min_dist = dist; + target_tree = *it; + tree_index = index; + if(verbose_) cout << "Database tree No." << tree_index << ": udpate min_dist: " << min_dist << endl; + } + } + + /* add new tree if necessary */ + if(new_tree) + { + if(!only_target) + { + TreeHandlePtr new_tree = TreeHandlePtr(new TreeHandle(nh_, nhp_, tree_pos)); + new_tree->setRadius(tree_radius); + add(new_tree); + return true; + } + } + else + { + /* update the global pos of the tree */ + target_tree->updatePos(tree_pos,false); + target_tree->setRadius(tree_radius); + if(verbose_) cout << "Database tree No." << tree_index << ": update, small diff:" << min_dist << endl; + return true; + } + + return false; +} + +void TreeDataBase::update() +{ + /* sort */ + std::sort(trees_.begin(),trees_.end(),std::greater()); + + if(verbose_) + { + cout << "update(sort): "; + for (vector::iterator it = trees_.begin(); it != trees_.end(); it++) + cout << (*it)->getVote() << ", "; + cout << endl; + } +} + +void TreeDataBase::visualization(std_msgs::Header header) +{ + visualization_msgs::MarkerArray msg; + + for (vector::iterator it = trees_.begin(); it != trees_.end(); it++) { + size_t index = distance(trees_.begin(), it); + + /* only show top level trees */ + if(index == valid_num_) break; + + visualization_msgs::Marker marker; + marker.header.frame_id = "world"; + marker.header.stamp = ros::Time::now(); + marker.ns = "tree_diameter"; + marker.id = index; + marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = (*it)->getPos().x(); + marker.pose.position.y = (*it)->getPos().y(); + marker.pose.position.z = 3.0; + marker.pose.orientation.w = 1.0; + marker.scale.z = 0.5; + marker.color.g = 1.0; + marker.color.a = 1.0; + ostringstream sout; + sout << fixed << setprecision(3) << (((*it)->getRadius()) * 2); + marker.text = sout.str(); + marker.lifetime = ros::Duration(); + msg.markers.push_back(marker); + + marker.ns = "tree"; + marker.type = visualization_msgs::Marker::CYLINDER; + marker.scale.z = 2.0; //tree height + marker.scale.x = marker.scale.y = (*it)->getRadius() * 2; + marker.pose.position.z = marker.scale.z / 2; + marker.color.r = 0.95; + marker.color.g = 0.59; + if(*it == center_tree_) marker.color.g = 0.1; //special color for center tree + marker.color.b = 0; + marker.color.a = 0.5; + msg.markers.push_back(marker); + } + pub_visualization_marker_.publish(msg); +} + + +void TreeDataBase::save() +{ + //boost::posix_time::ptime t = ros::Time::now().toBoost(); + boost::posix_time::ptime t = boost::posix_time::second_clock::local_time(); + boost::gregorian::date d = t.date(); + std::string date = boost::gregorian::to_iso_extended_string(d); + std::ostringstream h_os; h_os << t.time_of_day().hours(); + std::ostringstream m_os; m_os << t.time_of_day().minutes(); + + std::ofstream ofs; + ofs.open(std::getenv("HOME") + string("/.ros/") + + date + string("-") + h_os.str() + string("-") + + m_os.str() + string("-") + string("trees.yaml")); + + int center_index = getIndex(center_tree_); + ofs << "tree_num: " << (int)trees_.size() << " " << "center_tree: " << center_index << std::endl; + + for (vector::iterator it = trees_.begin(); it != trees_.end(); it++) + ofs << (*it)->getPos().x() << " " << (*it)->getPos().y() << " " << (*it)->getRadius() << " " << (*it)->getVote() << std::endl; + +} + +bool TreeDataBase::load(string file_name) +{ + std::ifstream ifs(file_name.c_str()); + + if(ifs.fail()) + { + ROS_ERROR("File do not exist"); + return false; + } + + std::string str; + std::stringstream ss_header; + std::string header1, header2; + int tree_num; + int center_tree_index; + + std::getline(ifs, str); + ss_header.str(str); + ss_header >> header1 >> tree_num >> header2 >> center_tree_index; + ROS_INFO("%s: %d, %s: %d", header1.c_str(), tree_num, header2.c_str(), center_tree_index); + + int all_vote = 0; + + /* get the tree data */ + for(int i = 0; i < tree_num; i++) + { + std::stringstream ss; + std::getline(ifs, str); + ss.str(str); + float x = 0, y = 0, radius = 0; + int vote = 0; + ss >> x >> y >> radius >> vote; + ROS_INFO("tree_pos: [%f, %f]; radius: %f; vote: %d", x, y, radius, vote); + TreeHandlePtr new_tree = TreeHandlePtr(new TreeHandle(nh_, nhp_, tf::Vector3(x, y, 0))); + new_tree->setRadius(radius); + new_tree->setVote(vote); + add(new_tree); + all_vote += vote; + + if(i == center_tree_index) center_tree_ = new_tree; + } + + /* filter trees */ + update(); //sort + int cut_vote = 0; + for (int i = tree_num - 1; i > 0; i--) { + cut_vote += trees_.at(i)->getVote(); + trees_.pop_back(); + if (cut_vote > all_vote * tree_cut_rate_) break; + } + valid_num_ = tree_num; + return true; +} + +int TreeDataBase::getIndex(TreeHandlePtr target_tree) +{ + int i = 0; + for (auto &tree : trees_) + { + if(tree == target_tree) return i; + i++; + } + return -1; +} + + + diff --git a/tree_tracking/src/tree_tracking.cpp b/tree_tracking/src/tree_tracking.cpp new file mode 100644 index 000000000..b0fccd1dc --- /dev/null +++ b/tree_tracking/src/tree_tracking.cpp @@ -0,0 +1,154 @@ +#include "tree_tracking/tree_tracking.h" +#include "tree_tracking/circle_detection.h" + +TreeTracking::TreeTracking(ros::NodeHandle nh, ros::NodeHandle nhp): + nh_(nh), nhp_(nhp), + tree_db_(nh, nhp) +{ + nhp_.param("urg_yaw_offset", urg_yaw_offset_, 0.0); + nhp_.param("laser_scan_topic_name", laser_scan_topic_name_, string("scan")); + nhp_.param("uav_odom_topic_name", odom_topic_name_, string("odom")); + nhp_.param("tree_radius_max", tree_radius_max_, 0.3); + nhp_.param("tree_radius_min", tree_radius_min_, 0.08); + nhp_.param("tree_scan_angle_thre", tree_scan_angle_thre_, 0.1); + nhp_.param("tree_circle_regulation_thre", tree_circle_regulation_thre_, 0.01); + nhp_.param("tree_global_location_topic_name", tree_global_location_topic_name_, string("tree_global_location")); + nhp_.param("visualization_marker_topic_name", visualization_marker_topic_name_, string("visualization_marker")); + + sub_laser_scan_ = nh_.subscribe(laser_scan_topic_name_, 1, &TreeTracking::laserScanCallback, this); + sub_odom_ = nh_.subscribe(odom_topic_name_, 1, &TreeTracking::uavOdomCallback, this); + pub_visualization_marker_ = nh_.advertise(visualization_marker_topic_name_, 1); +} + +// void TreeTracking::visionDetectionCallback(const geometry_msgs::Vector3StampedConstPtr& vision_detection_msg) +// { +// ROS_WARN("tree tracking: start tracking"); +// /* stop the vision detection */ +// std_msgs::Bool stop_msg; +// stop_msg.data = false; +// pub_stop_vision_detection_.publish(stop_msg); + +// tf::Matrix3x3 rotation; +// rotation.setRPY(0, 0, vision_detection_msg->vector.y + uav_yaw_ + urg_yaw_offset_); +// tf::Vector3 target_tree_global_location = uav_odom_ + rotation * tf::Vector3(vision_detection_msg->vector.z, 0, 0); +// initial_target_tree_direction_vec_ = rotation * tf::Vector3(vision_detection_msg->vector.z, 0, 0); +// initial_target_tree_direction_vec_ /= initial_target_tree_direction_vec_.length(); //normalize + +// /* start laesr-only subscribe */ +// sub_laser_scan_ = nh_.subscribe(laser_scan_topic_name_, 1, &TreeTracking::laserScanCallback, this); + +// /* add target tree to the tree data base */ +// TreeHandlePtr new_tree = TreeHandlePtr(new TreeHandle(nh_, nhp_, target_tree_global_location)); +// tree_db_.add(new_tree); +// tree_db_.setCenterTree(new_tree); +// target_trees_.resize(0); +// target_trees_.push_back(new_tree); + +// /* set the search center as the first target tree(with color marker) pos */ +// search_center_ = target_tree_global_location; + +// sub_vision_detection_.shutdown(); //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); + + /* we only consider in the 2D space */ + //uav_odom_.setZ(uav_msg->pose.pose.position.z); + uav_odom_.setZ(0); + uav_roll_ = r; uav_pitch_ = p; uav_yaw_ = y; +} + +void TreeTracking::laserScanCallback(const sensor_msgs::LaserScanConstPtr& scan_msg) +{ + // ROS_INFO("receive new laser scan"); + + /* extract the cluster */ + vector cluster_index; + for (size_t i = 0; i < scan_msg->ranges.size(); i++) + if(scan_msg->ranges[i] > 0) cluster_index.push_back(i); + + /* find the tree most close to the previous target tree */ + bool target_update = false; + // int prev_vote = target_trees_.back()->getVote(); + for ( vector::iterator it = cluster_index.begin(); it != cluster_index.end(); ++it) + { + // /* we do not update trees pos if there is big tilt */ + // if(fabs(uav_pitch_) > uav_tilt_thre_) + // { + // ROS_WARN("Too much tilt: %f", uav_pitch_); + // break; + // } + + tf::Vector3 tree_global_location; + + /* calculate the distance */ + tf::Matrix3x3 rotation; + rotation.setRPY(0, 0, *it * scan_msg->angle_increment + scan_msg->angle_min + uav_yaw_ + urg_yaw_offset_); + tree_global_location = uav_odom_ + rotation * tf::Vector3(scan_msg->ranges[*it], 0, 0); + + /* add tree to the database */ + // if(verbose_) cout << "Scan input tree No." << distance(cluster_index.begin(), it) << ": start update" << endl; + + /* calc radius with circle fitting */ + vector points; + int scan_point_num = 0; + for (int i = *it; !isnan(scan_msg->ranges[i]) && i < scan_msg->ranges.size(); i++) + { + double r = fabs(scan_msg->ranges[i]); + double theta = scan_msg->angle_min + (scan_msg->angle_increment) * i; + tf::Vector3 point(r * cos(theta), r * sin(theta), 0); + points.push_back(point); + scan_point_num++; + } + for (int i = (*it) - 1; !isnan(scan_msg->ranges[i]) && i > 0; i--) + { + double r = fabs(scan_msg->ranges[i]); + double theta = scan_msg->angle_min + (scan_msg->angle_increment) * i; + tf::Vector3 point(r * cos(theta), r * sin(theta), 0); + points.push_back(point); + scan_point_num++; + } + /* calc position and radius */ + tf::Vector3 tree_center_pos; double tree_radius, regulation; + CircleDetection::circleFitting(points, tree_center_pos, tree_radius, regulation); + /* angle filter */ + double scan_angle_real = scan_point_num * scan_msg->angle_increment; + double scan_angle_virtual = M_PI - 2 * acos(tree_radius / tree_center_pos.length()); + /* tree position filter */ + rotation.setRPY(0, 0, uav_yaw_ + urg_yaw_offset_); + tf::Vector3 tree_center_global_location = uav_odom_ + rotation * tf::Vector3(tree_center_pos.x(), tree_center_pos.y(), 0); + // tf::Vector3 initial_target_tree_pos = target_trees_.at(0)->getPos(); + // double projected_length_from_initial_target = initial_target_tree_direction_vec_.dot(tree_center_global_location - initial_target_tree_pos); + + + if (tree_radius > tree_radius_min_ && tree_radius < tree_radius_max_ && fabs((scan_angle_real - scan_angle_virtual) / scan_angle_real) < tree_scan_angle_thre_ && regulation < tree_circle_regulation_thre_) + { + target_update += tree_db_.updateSingleTree(tree_center_global_location, tree_radius, false); + } + else + { + // if(verbose_) + // ROS_INFO("radius: %f, min: %f, max: %f", tree_radius, tree_radius_min_, tree_radius_max_); + } + // else + // { + // target_update += tree_db_.updateSingleTree(tree_global_location, 0.2, only_target_); + // } + } + + /* update the whole database(sorting) */ + tree_db_.update(); + + /* send command for policy system */ + tree_db_.visualization(scan_msg->header); +} diff --git a/tree_tracking/src/tree_tracking_node.cpp b/tree_tracking/src/tree_tracking_node.cpp new file mode 100644 index 000000000..95ad17dc4 --- /dev/null +++ b/tree_tracking/src/tree_tracking_node.cpp @@ -0,0 +1,12 @@ +#include "tree_tracking/tree_tracking.h" + +int main (int argc, char **argv) +{ + ros::init (argc, argv, "tree_detector"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + TreeTracking* treeTrackingNode = new TreeTracking(nh, nh_private); + ros::spin (); + delete treeTrackingNode; + return 0; +}