From 7b9b95a3201028e4ce6e91624b3a87d563ffd394 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 29 Apr 2024 23:48:01 +0900 Subject: [PATCH 01/40] [hokuyo_estimation][build] init commit and evaluate by building code --- hokuyo_estimation/CMakeLists.txt | 54 ++++ hokuyo_estimation/include/circle_detection.h | 10 + hokuyo_estimation/include/tree_database.h | 93 +++++++ hokuyo_estimation/include/tree_tracking.h | 54 ++++ hokuyo_estimation/launch/laser_process.launch | 5 + hokuyo_estimation/package.xml | 53 ++++ hokuyo_estimation/src/circle_detection.cpp | 32 +++ hokuyo_estimation/src/tree_database.cpp | 249 ++++++++++++++++++ hokuyo_estimation/src/tree_tracking.cpp | 152 +++++++++++ hokuyo_estimation/src/tree_tracking_node.cpp | 12 + 10 files changed, 714 insertions(+) create mode 100644 hokuyo_estimation/CMakeLists.txt create mode 100644 hokuyo_estimation/include/circle_detection.h create mode 100644 hokuyo_estimation/include/tree_database.h create mode 100644 hokuyo_estimation/include/tree_tracking.h create mode 100644 hokuyo_estimation/launch/laser_process.launch create mode 100644 hokuyo_estimation/package.xml create mode 100644 hokuyo_estimation/src/circle_detection.cpp create mode 100644 hokuyo_estimation/src/tree_database.cpp create mode 100644 hokuyo_estimation/src/tree_tracking.cpp create mode 100644 hokuyo_estimation/src/tree_tracking_node.cpp diff --git a/hokuyo_estimation/CMakeLists.txt b/hokuyo_estimation/CMakeLists.txt new file mode 100644 index 000000000..b19fc8957 --- /dev/null +++ b/hokuyo_estimation/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.0.2) +project(hokuyo_estimation) + +## 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 +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +set(CMAKE_CXX_FLAGS "-std=c++11") + +find_package(OpenCV REQUIRED) +message(WARNING "OPENCV ${OpenCV_VERSION} FOUND") + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp + LIBRARIES tree_tracking +) + +########### +## Build ## +########### +include_directories( + include + ${OpenCV_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ) + +add_library(tree_tracking src/tree_tracking.cpp src/tree_database.cpp src/circle_detection.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) + +############# +## Install ## +############# diff --git a/hokuyo_estimation/include/circle_detection.h b/hokuyo_estimation/include/circle_detection.h new file mode 100644 index 000000000..f3aac855d --- /dev/null +++ b/hokuyo_estimation/include/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/hokuyo_estimation/include/tree_database.h b/hokuyo_estimation/include/tree_database.h new file mode 100644 index 000000000..9f6a57600 --- /dev/null +++ b/hokuyo_estimation/include/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/hokuyo_estimation/include/tree_tracking.h b/hokuyo_estimation/include/tree_tracking.h new file mode 100644 index 000000000..2bf8da265 --- /dev/null +++ b/hokuyo_estimation/include/tree_tracking.h @@ -0,0 +1,54 @@ +#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::Publisher pub_visualization_marker_; + string laser_scan_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/hokuyo_estimation/launch/laser_process.launch b/hokuyo_estimation/launch/laser_process.launch new file mode 100644 index 000000000..a9c9336b1 --- /dev/null +++ b/hokuyo_estimation/launch/laser_process.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/hokuyo_estimation/package.xml b/hokuyo_estimation/package.xml new file mode 100644 index 000000000..c552cd994 --- /dev/null +++ b/hokuyo_estimation/package.xml @@ -0,0 +1,53 @@ + + + hokuyo_estimation + 0.0.0 + The hokuyo_estimation package + + Haruki Kozuka + + BSD + + + catkin + cmake_modules + cv_bridge + dynamic_reconfigure + geometry_msgs + image_geometry + image_transport + nodelet + roscpp + sensor_msgs + std_msgs + tf + cmake_modules + cv_bridge + dynamic_reconfigure + geometry_msgs + image_geometry + image_transport + nodelet + roscpp + sensor_msgs + std_msgs + tf + cmake_modules + cv_bridge + dynamic_reconfigure + geometry_msgs + image_geometry + image_transport + nodelet + roscpp + sensor_msgs + std_msgs + tf + + + + + + + + diff --git a/hokuyo_estimation/src/circle_detection.cpp b/hokuyo_estimation/src/circle_detection.cpp new file mode 100644 index 000000000..59462ab22 --- /dev/null +++ b/hokuyo_estimation/src/circle_detection.cpp @@ -0,0 +1,32 @@ +#include "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/hokuyo_estimation/src/tree_database.cpp b/hokuyo_estimation/src/tree_database.cpp new file mode 100644 index 000000000..723e3e038 --- /dev/null +++ b/hokuyo_estimation/src/tree_database.cpp @@ -0,0 +1,249 @@ +#include "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/hokuyo_estimation/src/tree_tracking.cpp b/hokuyo_estimation/src/tree_tracking.cpp new file mode 100644 index 000000000..ddfb8d6d0 --- /dev/null +++ b/hokuyo_estimation/src/tree_tracking.cpp @@ -0,0 +1,152 @@ +#include "tree_tracking.h" +#include "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("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.005); + 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); + 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/hokuyo_estimation/src/tree_tracking_node.cpp b/hokuyo_estimation/src/tree_tracking_node.cpp new file mode 100644 index 000000000..78e26ec10 --- /dev/null +++ b/hokuyo_estimation/src/tree_tracking_node.cpp @@ -0,0 +1,12 @@ +#include "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; +} From 9f8dd65f799b5503970a5309b01793757a60124f Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Tue, 30 Apr 2024 17:06:57 +0900 Subject: [PATCH 02/40] [hokuyo_estimation] add odom subscriber --- hokuyo_estimation/include/tree_tracking.h | 3 ++- hokuyo_estimation/src/tree_tracking.cpp | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/hokuyo_estimation/include/tree_tracking.h b/hokuyo_estimation/include/tree_tracking.h index 2bf8da265..8fbe05dcb 100644 --- a/hokuyo_estimation/include/tree_tracking.h +++ b/hokuyo_estimation/include/tree_tracking.h @@ -30,8 +30,9 @@ class TreeTracking ros::NodeHandle nh_, nhp_; ros::Subscriber sub_laser_scan_; + ros::Subscriber sub_odom_; ros::Publisher pub_visualization_marker_; - string laser_scan_topic_name_; + string laser_scan_topic_name_, odom_topic_name_; string tree_global_location_topic_name_; string visualization_marker_topic_name_; diff --git a/hokuyo_estimation/src/tree_tracking.cpp b/hokuyo_estimation/src/tree_tracking.cpp index ddfb8d6d0..ad4265169 100644 --- a/hokuyo_estimation/src/tree_tracking.cpp +++ b/hokuyo_estimation/src/tree_tracking.cpp @@ -7,6 +7,7 @@ TreeTracking::TreeTracking(ros::NodeHandle nh, ros::NodeHandle 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("laser_scan_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); @@ -15,6 +16,7 @@ TreeTracking::TreeTracking(ros::NodeHandle nh, ros::NodeHandle nhp): 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); } From d205d7b3c88312330bba58be834fdf3a9043510a Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Tue, 30 Apr 2024 17:46:30 +0900 Subject: [PATCH 03/40] [hokuyo_estimation] add robot_ns in lanch file --- hokuyo_estimation/launch/laser_process.launch | 4 ++++ hokuyo_estimation/src/tree_database.cpp | 2 +- hokuyo_estimation/src/tree_tracking.cpp | 2 +- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/hokuyo_estimation/launch/laser_process.launch b/hokuyo_estimation/launch/laser_process.launch index a9c9336b1..f2cd35f97 100644 --- a/hokuyo_estimation/launch/laser_process.launch +++ b/hokuyo_estimation/launch/laser_process.launch @@ -1,5 +1,9 @@ + + + + diff --git a/hokuyo_estimation/src/tree_database.cpp b/hokuyo_estimation/src/tree_database.cpp index 723e3e038..6369ee204 100644 --- a/hokuyo_estimation/src/tree_database.cpp +++ b/hokuyo_estimation/src/tree_database.cpp @@ -33,7 +33,7 @@ TreeDataBase::TreeDataBase(ros::NodeHandle nh, ros::NodeHandle nhp):nh_(nh), nhp 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("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); } diff --git a/hokuyo_estimation/src/tree_tracking.cpp b/hokuyo_estimation/src/tree_tracking.cpp index ad4265169..640185ad2 100644 --- a/hokuyo_estimation/src/tree_tracking.cpp +++ b/hokuyo_estimation/src/tree_tracking.cpp @@ -13,7 +13,7 @@ TreeTracking::TreeTracking(ros::NodeHandle nh, ros::NodeHandle nhp): nhp_.param("tree_scan_angle_thre", tree_scan_angle_thre_, 0.1); nhp_.param("tree_circle_regulation_thre", tree_circle_regulation_thre_, 0.005); 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")); + 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); From 12dc417c358662d7dedbbeb0505e9ee34dd542e9 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Tue, 30 Apr 2024 18:01:27 +0900 Subject: [PATCH 04/40] [hokuyo_estimation] rename odom to match to jsk_aerial_robot --- hokuyo_estimation/launch/laser_process.launch | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/hokuyo_estimation/launch/laser_process.launch b/hokuyo_estimation/launch/laser_process.launch index f2cd35f97..7e78d0960 100644 --- a/hokuyo_estimation/launch/laser_process.launch +++ b/hokuyo_estimation/launch/laser_process.launch @@ -4,6 +4,8 @@ - + + + From 16832785b21836680c33d5e233c63040939d1f1e Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Tue, 30 Apr 2024 18:43:19 +0900 Subject: [PATCH 05/40] [hokuyo_estimation] delete prev_vote calculation --- hokuyo_estimation/src/tree_tracking.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hokuyo_estimation/src/tree_tracking.cpp b/hokuyo_estimation/src/tree_tracking.cpp index 640185ad2..dce28fb8d 100644 --- a/hokuyo_estimation/src/tree_tracking.cpp +++ b/hokuyo_estimation/src/tree_tracking.cpp @@ -79,7 +79,7 @@ void TreeTracking::laserScanCallback(const sensor_msgs::LaserScanConstPtr& scan_ /* find the tree most close to the previous target tree */ bool target_update = false; - int prev_vote = target_trees_.back()->getVote(); + // 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 */ From 77f6d7111f9c14673f04a8cddc1d1d485bb858ae Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Thu, 2 May 2024 15:14:21 +0900 Subject: [PATCH 06/40] [WIP][hokuyo_estimation][build ok] add filter system --- hokuyo_estimation/CMakeLists.txt | 3 + .../include/laser_clustering_filter.h | 181 ++++++++++++++++++ hokuyo_estimation/launch/laser_process.launch | 20 +- hokuyo_estimation/launch/tree_detector.launch | 16 ++ hokuyo_estimation/param/LaserClustering.yaml | 7 + .../param/LaserShadowFilter.yaml | 13 ++ .../src/laser_clustering_filter.cpp | 37 ++++ 7 files changed, 270 insertions(+), 7 deletions(-) create mode 100644 hokuyo_estimation/include/laser_clustering_filter.h create mode 100644 hokuyo_estimation/launch/tree_detector.launch create mode 100644 hokuyo_estimation/param/LaserClustering.yaml create mode 100644 hokuyo_estimation/param/LaserShadowFilter.yaml create mode 100644 hokuyo_estimation/src/laser_clustering_filter.cpp diff --git a/hokuyo_estimation/CMakeLists.txt b/hokuyo_estimation/CMakeLists.txt index b19fc8957..172e727e0 100644 --- a/hokuyo_estimation/CMakeLists.txt +++ b/hokuyo_estimation/CMakeLists.txt @@ -43,6 +43,9 @@ 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(tree_tracking src/tree_tracking.cpp src/tree_database.cpp src/circle_detection.cpp) target_link_libraries(tree_tracking ${catkin_LIBRARIES}) diff --git a/hokuyo_estimation/include/laser_clustering_filter.h b/hokuyo_estimation/include/laser_clustering_filter.h new file mode 100644 index 000000000..9cca024f2 --- /dev/null +++ b/hokuyo_estimation/include/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/hokuyo_estimation/launch/laser_process.launch b/hokuyo_estimation/launch/laser_process.launch index 7e78d0960..473eb683c 100644 --- a/hokuyo_estimation/launch/laser_process.launch +++ b/hokuyo_estimation/launch/laser_process.launch @@ -1,11 +1,17 @@ - - - - - - + + + + + + + + + + + + + - diff --git a/hokuyo_estimation/launch/tree_detector.launch b/hokuyo_estimation/launch/tree_detector.launch new file mode 100644 index 000000000..1f49026e3 --- /dev/null +++ b/hokuyo_estimation/launch/tree_detector.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/hokuyo_estimation/param/LaserClustering.yaml b/hokuyo_estimation/param/LaserClustering.yaml new file mode 100644 index 000000000..02286d044 --- /dev/null +++ b/hokuyo_estimation/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/hokuyo_estimation/param/LaserShadowFilter.yaml b/hokuyo_estimation/param/LaserShadowFilter.yaml new file mode 100644 index 000000000..9afc34b01 --- /dev/null +++ b/hokuyo_estimation/param/LaserShadowFilter.yaml @@ -0,0 +1,13 @@ +scan_filter_chain: +- name: shadows + type: laser_filters/ScanShadowsFilter + params: + min_angle: 10 + max_angle: 170 + neighbors: 10 + window: 1 +- name: range + type: laser_filters/LaserScanRangeFilter + params: + lower_threshold: 0.3 + upper_threshold: 15 diff --git a/hokuyo_estimation/src/laser_clustering_filter.cpp b/hokuyo_estimation/src/laser_clustering_filter.cpp new file mode 100644 index 000000000..9fb247ec0 --- /dev/null +++ b/hokuyo_estimation/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 "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) From d69280fb7f9aad440bd8c4e012a578488c26ade8 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 4 May 2024 16:18:18 +0900 Subject: [PATCH 07/40] [hokuyo_estimation] change include path from jsk_uav_forest_perception to hokuyo_estimation --- hokuyo_estimation/launch/laser_process.launch | 4 ++-- hokuyo_estimation/launch/tree_detector.launch | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/hokuyo_estimation/launch/laser_process.launch b/hokuyo_estimation/launch/laser_process.launch index 473eb683c..291929ad0 100644 --- a/hokuyo_estimation/launch/laser_process.launch +++ b/hokuyo_estimation/launch/laser_process.launch @@ -4,14 +4,14 @@ - + - + diff --git a/hokuyo_estimation/launch/tree_detector.launch b/hokuyo_estimation/launch/tree_detector.launch index 1f49026e3..b66373623 100644 --- a/hokuyo_estimation/launch/tree_detector.launch +++ b/hokuyo_estimation/launch/tree_detector.launch @@ -5,7 +5,7 @@ - + From 1c0f43a4cfef9adaf0dd8c23196176923f9e61f0 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 4 May 2024 18:33:36 +0900 Subject: [PATCH 08/40] [WIP][hokuyo_estimation][launch OK] add filter package to inclde filter package in build and exec --- hokuyo_estimation/laser_filters_plugins.xml | 10 ++++++++++ hokuyo_estimation/package.xml | 12 ++++++++++-- 2 files changed, 20 insertions(+), 2 deletions(-) create mode 100644 hokuyo_estimation/laser_filters_plugins.xml diff --git a/hokuyo_estimation/laser_filters_plugins.xml b/hokuyo_estimation/laser_filters_plugins.xml new file mode 100644 index 000000000..f1aca41fb --- /dev/null +++ b/hokuyo_estimation/laser_filters_plugins.xml @@ -0,0 +1,10 @@ + + + + + This is a filter which do clustering from a laser. + + + + diff --git a/hokuyo_estimation/package.xml b/hokuyo_estimation/package.xml index c552cd994..d9683b016 100644 --- a/hokuyo_estimation/package.xml +++ b/hokuyo_estimation/package.xml @@ -21,6 +21,9 @@ sensor_msgs std_msgs tf + laser_filters + filters + cmake_modules cv_bridge dynamic_reconfigure @@ -32,6 +35,9 @@ sensor_msgs std_msgs tf + laser_filters + filters + cmake_modules cv_bridge dynamic_reconfigure @@ -43,11 +49,13 @@ sensor_msgs std_msgs tf + laser_filters + filters - - + + From 9dcb82760c1913e4e26e2d58b22a912e571537f3 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 4 May 2024 18:46:26 +0900 Subject: [PATCH 09/40] [hokuyo_estimation] rename subscribing laser topic name of tree_tracking_node from scan to scan_clustered to get filtered scan data --- hokuyo_estimation/launch/tree_detector.launch | 1 + hokuyo_estimation/src/tree_tracking.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/hokuyo_estimation/launch/tree_detector.launch b/hokuyo_estimation/launch/tree_detector.launch index b66373623..5f1505d44 100644 --- a/hokuyo_estimation/launch/tree_detector.launch +++ b/hokuyo_estimation/launch/tree_detector.launch @@ -10,6 +10,7 @@ + diff --git a/hokuyo_estimation/src/tree_tracking.cpp b/hokuyo_estimation/src/tree_tracking.cpp index dce28fb8d..8026f5d02 100644 --- a/hokuyo_estimation/src/tree_tracking.cpp +++ b/hokuyo_estimation/src/tree_tracking.cpp @@ -7,7 +7,7 @@ TreeTracking::TreeTracking(ros::NodeHandle nh, ros::NodeHandle 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("laser_scan_topic_name", odom_topic_name_, string("odom")); + 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); From 8e9aea0bc06ae5e0bab901e3b820e261f7eaba20 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Tue, 7 May 2024 16:24:27 +0900 Subject: [PATCH 10/40] [hokuyo_estimation] add change config file to notice in different obstacles --- hokuyo_estimation/param/LaserShadowFilter.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hokuyo_estimation/param/LaserShadowFilter.yaml b/hokuyo_estimation/param/LaserShadowFilter.yaml index 9afc34b01..4f3387955 100644 --- a/hokuyo_estimation/param/LaserShadowFilter.yaml +++ b/hokuyo_estimation/param/LaserShadowFilter.yaml @@ -2,12 +2,12 @@ scan_filter_chain: - name: shadows type: laser_filters/ScanShadowsFilter params: - min_angle: 10 - max_angle: 170 + min_angle: 20 + max_angle: 160 neighbors: 10 window: 1 - name: range type: laser_filters/LaserScanRangeFilter params: - lower_threshold: 0.3 + lower_threshold: 0.2 upper_threshold: 15 From f45ad0c815718473a0e7ffe9ab208482a19bf555 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 20 May 2024 21:36:06 +0900 Subject: [PATCH 11/40] [hokuyo_estimation] show rviz data --- hokuyo_estimation/config/rviz_config.rviz | 208 ++++++++++++++++++ hokuyo_estimation/launch/tree_detector.launch | 4 + 2 files changed, 212 insertions(+) create mode 100644 hokuyo_estimation/config/rviz_config.rviz diff --git a/hokuyo_estimation/config/rviz_config.rviz b/hokuyo_estimation/config/rviz_config.rviz new file mode 100644 index 000000000..b6bc2f59e --- /dev/null +++ b/hokuyo_estimation/config/rviz_config.rviz @@ -0,0 +1,208 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /LaserScan1 + - /LaserScan1/Status1 + - /LaserScan2 + - /LaserScan3 + Splitter Ratio: 0.45338207483291626 + Tree Height: 1799 + - 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: false + 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: false + - 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 + - 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: false + 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: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: laser + 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: 8.846917152404785 + 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: 2.400782585144043 + Y: -1.2781561613082886 + Z: 5.178714275360107 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Yaw: 3.1435818672180176 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2096 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000022500000792fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000792000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000792fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000792000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000089d0000003efc0100000002fb0000000800540069006d006501000000000000089d000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000056c0000079200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2205 + X: 4195 + Y: 27 diff --git a/hokuyo_estimation/launch/tree_detector.launch b/hokuyo_estimation/launch/tree_detector.launch index 5f1505d44..304d6a354 100644 --- a/hokuyo_estimation/launch/tree_detector.launch +++ b/hokuyo_estimation/launch/tree_detector.launch @@ -2,6 +2,7 @@ + @@ -13,5 +14,8 @@ + + + From b189eff39aaa455b8af923c1dffbdbf8bf26ed95 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 20 May 2024 21:46:14 +0900 Subject: [PATCH 12/40] [hokuyo_estimation] show result of the tree --- hokuyo_estimation/config/rviz_config.rviz | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/hokuyo_estimation/config/rviz_config.rviz b/hokuyo_estimation/config/rviz_config.rviz index b6bc2f59e..288c9348a 100644 --- a/hokuyo_estimation/config/rviz_config.rviz +++ b/hokuyo_estimation/config/rviz_config.rviz @@ -8,9 +8,10 @@ Panels: - /Status1 - /Grid1 - /LaserScan1 - - /LaserScan1/Status1 - /LaserScan2 - /LaserScan3 + - /MarkerArray1 + - /MarkerArray1/Status1 Splitter Ratio: 0.45338207483291626 Tree Height: 1799 - Class: rviz/Selection @@ -68,7 +69,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -83,7 +84,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -124,7 +125,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -139,7 +140,15 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /multirotor/visualization_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 From 13c03272bbb7e4ccd3a2afae83b18ac89e9c128e Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 20 May 2024 21:57:03 +0900 Subject: [PATCH 13/40] [hokuyo_estimation] add odom data --- hokuyo_estimation/config/rviz_config.rviz | 59 +++++++++++++++++++---- 1 file changed, 49 insertions(+), 10 deletions(-) diff --git a/hokuyo_estimation/config/rviz_config.rviz b/hokuyo_estimation/config/rviz_config.rviz index 288c9348a..feb570abb 100644 --- a/hokuyo_estimation/config/rviz_config.rviz +++ b/hokuyo_estimation/config/rviz_config.rviz @@ -12,6 +12,9 @@ Panels: - /LaserScan3 - /MarkerArray1 - /MarkerArray1/Status1 + - /Odometry1 + - /Odometry1/Status1 + - /Odometry1/Shape1 Splitter Ratio: 0.45338207483291626 Tree Height: 1799 - Class: rviz/Selection @@ -146,14 +149,50 @@ Visualization Manager: Marker Topic: /multirotor/visualization_marker Name: MarkerArray Namespaces: - {} + tree: true + tree_diameter: true Queue Size: 100 Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /multirotor/uav/cog/odom + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: laser + Fixed Frame: world Frame Rate: 30 Name: root Tools: @@ -177,7 +216,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 8.846917152404785 + Distance: 15.894111633300781 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -185,9 +224,9 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 2.400782585144043 - Y: -1.2781561613082886 - Z: 5.178714275360107 + X: 2.302905321121216 + Y: -0.2775145471096039 + Z: 5.178598403930664 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -195,7 +234,7 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 1.5697963237762451 Target Frame: - Yaw: 3.1435818672180176 + Yaw: 3.123581886291504 Saved: ~ Window Geometry: Displays: @@ -203,7 +242,7 @@ Window Geometry: Height: 2096 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000022500000792fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000792000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000792fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000792000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000089d0000003efc0100000002fb0000000800540069006d006501000000000000089d000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000056c0000079200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002e100000792fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000792000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000792fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000792000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000091a0000003efc0100000002fb0000000800540069006d006501000000000000091a000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000052d0000079200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -212,6 +251,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2205 - X: 4195 + Width: 2330 + X: 2626 Y: 27 From 2323214e97a14e05475e20fc06544e773eae31e2 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 20 May 2024 23:47:45 +0900 Subject: [PATCH 14/40] [hokuyo_estimation] show odom and tree data --- hokuyo_estimation/config/rviz_config.rviz | 48 +++++------------------ 1 file changed, 10 insertions(+), 38 deletions(-) diff --git a/hokuyo_estimation/config/rviz_config.rviz b/hokuyo_estimation/config/rviz_config.rviz index feb570abb..796458916 100644 --- a/hokuyo_estimation/config/rviz_config.rviz +++ b/hokuyo_estimation/config/rviz_config.rviz @@ -9,12 +9,10 @@ Panels: - /Grid1 - /LaserScan1 - /LaserScan2 + - /LaserScan2/Status1 - /LaserScan3 - /MarkerArray1 - - /MarkerArray1/Status1 - - /Odometry1 - - /Odometry1/Status1 - - /Odometry1/Shape1 + - /Axes1 Splitter Ratio: 0.45338207483291626 Tree Height: 1799 - Class: rviz/Selection @@ -34,7 +32,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: LaserScan + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: @@ -153,40 +151,14 @@ Visualization Manager: tree_diameter: true Queue Size: 100 Value: true - - Angle Tolerance: 0.10000000149011612 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true + - Alpha: 1 + Class: rviz/Axes Enabled: true - Keep: 100 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Queue Size: 10 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Arrow - Topic: /multirotor/uav/cog/odom - Unreliable: false + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: multirotor/main_body + Show Trail: false Value: true Enabled: true Global Options: From 57c5056e26f990c4d2d68cc356482f4fbe2211dd Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Tue, 21 May 2024 00:49:48 +0900 Subject: [PATCH 15/40] [hokuyo_estimation] fix position of rviz --- hokuyo_estimation/config/rviz_config.rviz | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/hokuyo_estimation/config/rviz_config.rviz b/hokuyo_estimation/config/rviz_config.rviz index 796458916..79084b7ff 100644 --- a/hokuyo_estimation/config/rviz_config.rviz +++ b/hokuyo_estimation/config/rviz_config.rviz @@ -9,7 +9,6 @@ Panels: - /Grid1 - /LaserScan1 - /LaserScan2 - - /LaserScan2/Status1 - /LaserScan3 - /MarkerArray1 - /Axes1 @@ -32,7 +31,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: LaserScan Preferences: PromptSaveOnExit: true Toolbars: @@ -109,7 +108,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Flat Squares - Topic: /multirotor/scan_clustered + Topic: /multirotor/shadow_filtered Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -137,7 +136,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Flat Squares - Topic: /multirotor/shadow_filtered + Topic: /multirotor/scan_clustered Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -188,7 +187,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 15.894111633300781 + Distance: 11.88591194152832 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -196,17 +195,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 2.302905321121216 - Y: -0.2775145471096039 - Z: 5.178598403930664 + X: 1.95753812789917 + Y: -0.37241891026496887 + Z: 2.0585882663726807 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 + Pitch: 0.1597970724105835 Target Frame: - Yaw: 3.123581886291504 + Yaw: 2.558580160140991 Saved: ~ Window Geometry: Displays: @@ -214,7 +213,7 @@ Window Geometry: Height: 2096 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002e100000792fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000792000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000792fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000792000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000091a0000003efc0100000002fb0000000800540069006d006501000000000000091a000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000052d0000079200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000021900000792fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000792000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000792fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000792000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a750000003efc0100000002fb0000000800540069006d0065010000000000000a75000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000007500000079200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -223,6 +222,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2330 + Width: 2677 X: 2626 Y: 27 From 30560eef79b8504a01d1dd4f51ca5ae8f1a409a1 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Wed, 22 May 2024 18:33:52 +0900 Subject: [PATCH 16/40] [hokuyo_estimation] change to easy to watch lider output --- hokuyo_estimation/config/rviz_config.rviz | 24 +++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/hokuyo_estimation/config/rviz_config.rviz b/hokuyo_estimation/config/rviz_config.rviz index 79084b7ff..3d412c49d 100644 --- a/hokuyo_estimation/config/rviz_config.rviz +++ b/hokuyo_estimation/config/rviz_config.rviz @@ -13,7 +13,7 @@ Panels: - /MarkerArray1 - /Axes1 Splitter Ratio: 0.45338207483291626 - Tree Height: 1799 + Tree Height: 1266 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -187,7 +187,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 11.88591194152832 + Distance: 12.02877140045166 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -195,25 +195,25 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 1.95753812789917 - Y: -0.37241891026496887 - Z: 2.0585882663726807 + X: 1.8027316331863403 + Y: -0.8663604855537415 + Z: -0.2962363660335541 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.1597970724105835 + Pitch: 0.9797967076301575 Target Frame: - Yaw: 2.558580160140991 + Yaw: 3.003582000732422 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 2096 + Height: 1563 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000021900000792fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000792000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000792fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000792000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a750000003efc0100000002fb0000000800540069006d0065010000000000000a75000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000007500000079200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002190000057dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000057d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001000000057dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000057d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009be0000003efc0100000002fb0000000800540069006d00650100000000000009be000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006990000057d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -222,6 +222,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2677 - X: 2626 - Y: 27 + Width: 2494 + X: 66 + Y: 0 From fbe2e659421cf4d4d5fc854d298f9aca7dcc75b8 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Thu, 23 May 2024 13:12:40 +0900 Subject: [PATCH 17/40] [hokuyo_estimation][LaserShadowFilter][gain tuning] percept close obstacle to quadrotor --- hokuyo_estimation/param/LaserShadowFilter.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hokuyo_estimation/param/LaserShadowFilter.yaml b/hokuyo_estimation/param/LaserShadowFilter.yaml index 4f3387955..2e83e0ff8 100644 --- a/hokuyo_estimation/param/LaserShadowFilter.yaml +++ b/hokuyo_estimation/param/LaserShadowFilter.yaml @@ -4,8 +4,8 @@ scan_filter_chain: params: min_angle: 20 max_angle: 160 - neighbors: 10 - window: 1 + neighbors: 3 + window: 3 - name: range type: laser_filters/LaserScanRangeFilter params: From dc9d1775f1114c2057ebe51efd105439ba9eac17 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Thu, 23 May 2024 14:54:34 +0900 Subject: [PATCH 18/40] [hokuyo_estimation][from rosbag data] change LaserShadowFilter param & tree_circle_regulation_thre to fit from rosbag data --- hokuyo_estimation/param/LaserShadowFilter.yaml | 4 ++-- hokuyo_estimation/src/tree_tracking.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/hokuyo_estimation/param/LaserShadowFilter.yaml b/hokuyo_estimation/param/LaserShadowFilter.yaml index 2e83e0ff8..a51e66422 100644 --- a/hokuyo_estimation/param/LaserShadowFilter.yaml +++ b/hokuyo_estimation/param/LaserShadowFilter.yaml @@ -2,8 +2,8 @@ scan_filter_chain: - name: shadows type: laser_filters/ScanShadowsFilter params: - min_angle: 20 - max_angle: 160 + min_angle: 10 + max_angle: 170 neighbors: 3 window: 3 - name: range diff --git a/hokuyo_estimation/src/tree_tracking.cpp b/hokuyo_estimation/src/tree_tracking.cpp index 8026f5d02..973f7c60e 100644 --- a/hokuyo_estimation/src/tree_tracking.cpp +++ b/hokuyo_estimation/src/tree_tracking.cpp @@ -11,7 +11,7 @@ TreeTracking::TreeTracking(ros::NodeHandle nh, ros::NodeHandle nhp): 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.005); + 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")); From ca9c8937a43586adc87863627611b86311a73725 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Thu, 23 May 2024 17:31:00 +0900 Subject: [PATCH 19/40] [hokuyo_estimation] make visualize launch file to check estimation in real experiment --- .../launch/tree_detector_visualize.launch | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 hokuyo_estimation/launch/tree_detector_visualize.launch diff --git a/hokuyo_estimation/launch/tree_detector_visualize.launch b/hokuyo_estimation/launch/tree_detector_visualize.launch new file mode 100644 index 000000000..05474953f --- /dev/null +++ b/hokuyo_estimation/launch/tree_detector_visualize.launch @@ -0,0 +1,10 @@ + + + + + + + + + + From 0f947b1f713677006901ad47cd888336ed3aaa1d Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Thu, 23 May 2024 21:21:09 +0900 Subject: [PATCH 20/40] [hokuyo_estimation] bringup hokuyo_estimation from bringup.launch in agile_multirotor --- hokuyo_estimation/launch/bringup.launch | 20 +++++++++++++++++++ hokuyo_estimation/launch/tree_detector.launch | 7 ++----- hokuyo_estimation/launch/urg_node.launch | 10 ++++++++++ 3 files changed, 32 insertions(+), 5 deletions(-) create mode 100644 hokuyo_estimation/launch/bringup.launch create mode 100644 hokuyo_estimation/launch/urg_node.launch diff --git a/hokuyo_estimation/launch/bringup.launch b/hokuyo_estimation/launch/bringup.launch new file mode 100644 index 000000000..147005e49 --- /dev/null +++ b/hokuyo_estimation/launch/bringup.launch @@ -0,0 +1,20 @@ + + + ########### launch config ########### + + + + + + + + + + + + + + + + + diff --git a/hokuyo_estimation/launch/tree_detector.launch b/hokuyo_estimation/launch/tree_detector.launch index 304d6a354..bd30d4824 100644 --- a/hokuyo_estimation/launch/tree_detector.launch +++ b/hokuyo_estimation/launch/tree_detector.launch @@ -1,9 +1,7 @@ - - - - + + @@ -17,5 +15,4 @@ - diff --git a/hokuyo_estimation/launch/urg_node.launch b/hokuyo_estimation/launch/urg_node.launch new file mode 100644 index 000000000..31a89d54c --- /dev/null +++ b/hokuyo_estimation/launch/urg_node.launch @@ -0,0 +1,10 @@ + + + + + + + + + + From 6a8ce3791404a3cccc73e072b169bd1df3a8f7e5 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Fri, 24 May 2024 10:26:29 +0900 Subject: [PATCH 21/40] [hokuyo_estimation] comment out ROS_INFO for this is already debugged --- hokuyo_estimation/src/tree_tracking.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hokuyo_estimation/src/tree_tracking.cpp b/hokuyo_estimation/src/tree_tracking.cpp index 973f7c60e..a04754f77 100644 --- a/hokuyo_estimation/src/tree_tracking.cpp +++ b/hokuyo_estimation/src/tree_tracking.cpp @@ -70,7 +70,7 @@ void TreeTracking::uavOdomCallback(const nav_msgs::OdometryConstPtr& uav_msg) void TreeTracking::laserScanCallback(const sensor_msgs::LaserScanConstPtr& scan_msg) { - ROS_INFO("receive new laser scan"); + // ROS_INFO("receive new laser scan"); /* extract the cluster */ vector cluster_index; @@ -138,7 +138,7 @@ void TreeTracking::laserScanCallback(const sensor_msgs::LaserScanConstPtr& scan_ else { // if(verbose_) - ROS_INFO("radius: %f, min: %f, max: %f", tree_radius, tree_radius_min_, tree_radius_max_); + // ROS_INFO("radius: %f, min: %f, max: %f", tree_radius, tree_radius_min_, tree_radius_max_); } // else // { From 5f64d2a37b585c61049deac9779af6356f6b293a Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Tue, 4 Jun 2024 21:12:53 +0900 Subject: [PATCH 22/40] [hokuyo_estimation][debug] able to scan in hokuyo_sensor --- hokuyo_estimation/launch/bringup.launch | 1 + hokuyo_estimation/launch/urg_node.launch | 2 -- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/hokuyo_estimation/launch/bringup.launch b/hokuyo_estimation/launch/bringup.launch index 147005e49..974b62274 100644 --- a/hokuyo_estimation/launch/bringup.launch +++ b/hokuyo_estimation/launch/bringup.launch @@ -8,6 +8,7 @@ + diff --git a/hokuyo_estimation/launch/urg_node.launch b/hokuyo_estimation/launch/urg_node.launch index 31a89d54c..27f4c8d3c 100644 --- a/hokuyo_estimation/launch/urg_node.launch +++ b/hokuyo_estimation/launch/urg_node.launch @@ -1,10 +1,8 @@ - - From fe76e17998e6a87741614039b4685bcd8492a8ab Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 16:36:11 +0900 Subject: [PATCH 23/40] [hokuyo_estimation] show tree_info easily for real experiment --- hokuyo_estimation/config/rviz_config.rviz | 30 +++++++++++------------ 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/hokuyo_estimation/config/rviz_config.rviz b/hokuyo_estimation/config/rviz_config.rviz index 3d412c49d..0c9e059fa 100644 --- a/hokuyo_estimation/config/rviz_config.rviz +++ b/hokuyo_estimation/config/rviz_config.rviz @@ -13,7 +13,7 @@ Panels: - /MarkerArray1 - /Axes1 Splitter Ratio: 0.45338207483291626 - Tree Height: 1266 + Tree Height: 1239 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -195,25 +195,25 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 1.8027316331863403 - Y: -0.8663604855537415 - Z: -0.2962363660335541 + 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: 0.9797967076301575 + Pitch: 1.14479660987854 Target Frame: - Yaw: 3.003582000732422 + Yaw: 3.213582992553711 Saved: ~ Window Geometry: Displays: - collapsed: false - Height: 1563 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002190000057dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000057d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001000000057dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000057d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009be0000003efc0100000002fb0000000800540069006d00650100000000000009be000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006990000057d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + collapsed: true + Height: 1536 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000021900000562fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000562000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010000000562fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000562000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003f10000003efc0100000002fb0000000800540069006d00650100000000000003f1000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003f10000056200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -221,7 +221,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false - Width: 2494 - X: 66 - Y: 0 + collapsed: true + Width: 1009 + X: 1551 + Y: 27 From bb59ee2c7507f0fa8d720b0fd1c9533eaddbde85 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 21:08:55 +0900 Subject: [PATCH 24/40] [hokuyo_estimation] add laser_filters and filters in components --- hokuyo_estimation/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hokuyo_estimation/CMakeLists.txt b/hokuyo_estimation/CMakeLists.txt index 172e727e0..a6d3b3587 100644 --- a/hokuyo_estimation/CMakeLists.txt +++ b/hokuyo_estimation/CMakeLists.txt @@ -19,6 +19,8 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs tf + laser_filters + filters ) ## System dependencies are found with CMake's conventions From 6fb91b9276551a256e4c0058c70dd865bcd88587 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 21:29:08 +0900 Subject: [PATCH 25/40] [hokuyo_estimation] not overwritten CMAKE_CXX_FLAGS --- hokuyo_estimation/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hokuyo_estimation/CMakeLists.txt b/hokuyo_estimation/CMakeLists.txt index a6d3b3587..59b5ceb21 100644 --- a/hokuyo_estimation/CMakeLists.txt +++ b/hokuyo_estimation/CMakeLists.txt @@ -25,7 +25,7 @@ find_package(catkin REQUIRED COMPONENTS ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -set(CMAKE_CXX_FLAGS "-std=c++11") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") find_package(OpenCV REQUIRED) message(WARNING "OPENCV ${OpenCV_VERSION} FOUND") From eb55ed500798195be9605f32b3bc440b2ce7e239 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 21:33:06 +0900 Subject: [PATCH 26/40] [hokuyo_estimation] add CATKIN_DEPENDS from github action --- hokuyo_estimation/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hokuyo_estimation/CMakeLists.txt b/hokuyo_estimation/CMakeLists.txt index 59b5ceb21..2cade4963 100644 --- a/hokuyo_estimation/CMakeLists.txt +++ b/hokuyo_estimation/CMakeLists.txt @@ -32,7 +32,7 @@ message(WARNING "OPENCV ${OpenCV_VERSION} FOUND") catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS roscpp + CATKIN_DEPENDS roscpp sensor_msgs nodelet tf std_msgs dynamic_reconfigure geometry_msgs LIBRARIES tree_tracking ) From 77e552dda4aa7ca131ac7866a03efad0a213b144 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 21:40:59 +0900 Subject: [PATCH 27/40] [hokuyo_estimation] install plugin file in CmakeLists --- hokuyo_estimation/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/hokuyo_estimation/CMakeLists.txt b/hokuyo_estimation/CMakeLists.txt index 2cade4963..cc1647113 100644 --- a/hokuyo_estimation/CMakeLists.txt +++ b/hokuyo_estimation/CMakeLists.txt @@ -57,3 +57,6 @@ target_link_libraries (tree_tracking_node ${catkin_LIBRARIES} tree_tracking) ############# ## Install ## ############# +install(FILES laser_filters_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) From 128eb40da86901f2e588a7431b728030d289d560 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 21:50:39 +0900 Subject: [PATCH 28/40] [hokuyo_estimation] delete tree_tracking in LIBRARIES variable for this is made in their build --- hokuyo_estimation/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hokuyo_estimation/CMakeLists.txt b/hokuyo_estimation/CMakeLists.txt index cc1647113..ac0d556cf 100644 --- a/hokuyo_estimation/CMakeLists.txt +++ b/hokuyo_estimation/CMakeLists.txt @@ -33,7 +33,7 @@ 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 + # LIBRARIES tree_tracking ) ########### From 269841d8792e2f5a1a582d44c8bc0ebe7b17573c Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 22:10:00 +0900 Subject: [PATCH 29/40] [hokuyo_estimation] rename package name from hokuyo_estimation to tree_tracking for this is sutable for express my PR --- {hokuyo_estimation => tree_tracking}/CMakeLists.txt | 2 +- {hokuyo_estimation => tree_tracking}/config/rviz_config.rviz | 0 .../include/tree_tracking}/circle_detection.h | 0 .../include/tree_tracking}/laser_clustering_filter.h | 0 .../include/tree_tracking}/tree_database.h | 0 .../include/tree_tracking}/tree_tracking.h | 0 .../laser_filters_plugins.xml | 0 {hokuyo_estimation => tree_tracking}/launch/bringup.launch | 0 .../launch/laser_process.launch | 0 .../launch/tree_detector.launch | 0 .../launch/tree_detector_visualize.launch | 0 {hokuyo_estimation => tree_tracking}/launch/urg_node.launch | 0 {hokuyo_estimation => tree_tracking}/package.xml | 4 ++-- .../param/LaserClustering.yaml | 0 .../param/LaserShadowFilter.yaml | 0 {hokuyo_estimation => tree_tracking}/src/circle_detection.cpp | 0 .../src/laser_clustering_filter.cpp | 0 {hokuyo_estimation => tree_tracking}/src/tree_database.cpp | 0 {hokuyo_estimation => tree_tracking}/src/tree_tracking.cpp | 0 .../src/tree_tracking_node.cpp | 0 20 files changed, 3 insertions(+), 3 deletions(-) rename {hokuyo_estimation => tree_tracking}/CMakeLists.txt (98%) rename {hokuyo_estimation => tree_tracking}/config/rviz_config.rviz (100%) rename {hokuyo_estimation/include => tree_tracking/include/tree_tracking}/circle_detection.h (100%) rename {hokuyo_estimation/include => tree_tracking/include/tree_tracking}/laser_clustering_filter.h (100%) rename {hokuyo_estimation/include => tree_tracking/include/tree_tracking}/tree_database.h (100%) rename {hokuyo_estimation/include => tree_tracking/include/tree_tracking}/tree_tracking.h (100%) rename {hokuyo_estimation => tree_tracking}/laser_filters_plugins.xml (100%) rename {hokuyo_estimation => tree_tracking}/launch/bringup.launch (100%) rename {hokuyo_estimation => tree_tracking}/launch/laser_process.launch (100%) rename {hokuyo_estimation => tree_tracking}/launch/tree_detector.launch (100%) rename {hokuyo_estimation => tree_tracking}/launch/tree_detector_visualize.launch (100%) rename {hokuyo_estimation => tree_tracking}/launch/urg_node.launch (100%) rename {hokuyo_estimation => tree_tracking}/package.xml (96%) rename {hokuyo_estimation => tree_tracking}/param/LaserClustering.yaml (100%) rename {hokuyo_estimation => tree_tracking}/param/LaserShadowFilter.yaml (100%) rename {hokuyo_estimation => tree_tracking}/src/circle_detection.cpp (100%) rename {hokuyo_estimation => tree_tracking}/src/laser_clustering_filter.cpp (100%) rename {hokuyo_estimation => tree_tracking}/src/tree_database.cpp (100%) rename {hokuyo_estimation => tree_tracking}/src/tree_tracking.cpp (100%) rename {hokuyo_estimation => tree_tracking}/src/tree_tracking_node.cpp (100%) diff --git a/hokuyo_estimation/CMakeLists.txt b/tree_tracking/CMakeLists.txt similarity index 98% rename from hokuyo_estimation/CMakeLists.txt rename to tree_tracking/CMakeLists.txt index ac0d556cf..53581e49b 100644 --- a/hokuyo_estimation/CMakeLists.txt +++ b/tree_tracking/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(hokuyo_estimation) +project(tree_tracking) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) diff --git a/hokuyo_estimation/config/rviz_config.rviz b/tree_tracking/config/rviz_config.rviz similarity index 100% rename from hokuyo_estimation/config/rviz_config.rviz rename to tree_tracking/config/rviz_config.rviz diff --git a/hokuyo_estimation/include/circle_detection.h b/tree_tracking/include/tree_tracking/circle_detection.h similarity index 100% rename from hokuyo_estimation/include/circle_detection.h rename to tree_tracking/include/tree_tracking/circle_detection.h diff --git a/hokuyo_estimation/include/laser_clustering_filter.h b/tree_tracking/include/tree_tracking/laser_clustering_filter.h similarity index 100% rename from hokuyo_estimation/include/laser_clustering_filter.h rename to tree_tracking/include/tree_tracking/laser_clustering_filter.h diff --git a/hokuyo_estimation/include/tree_database.h b/tree_tracking/include/tree_tracking/tree_database.h similarity index 100% rename from hokuyo_estimation/include/tree_database.h rename to tree_tracking/include/tree_tracking/tree_database.h diff --git a/hokuyo_estimation/include/tree_tracking.h b/tree_tracking/include/tree_tracking/tree_tracking.h similarity index 100% rename from hokuyo_estimation/include/tree_tracking.h rename to tree_tracking/include/tree_tracking/tree_tracking.h diff --git a/hokuyo_estimation/laser_filters_plugins.xml b/tree_tracking/laser_filters_plugins.xml similarity index 100% rename from hokuyo_estimation/laser_filters_plugins.xml rename to tree_tracking/laser_filters_plugins.xml diff --git a/hokuyo_estimation/launch/bringup.launch b/tree_tracking/launch/bringup.launch similarity index 100% rename from hokuyo_estimation/launch/bringup.launch rename to tree_tracking/launch/bringup.launch diff --git a/hokuyo_estimation/launch/laser_process.launch b/tree_tracking/launch/laser_process.launch similarity index 100% rename from hokuyo_estimation/launch/laser_process.launch rename to tree_tracking/launch/laser_process.launch diff --git a/hokuyo_estimation/launch/tree_detector.launch b/tree_tracking/launch/tree_detector.launch similarity index 100% rename from hokuyo_estimation/launch/tree_detector.launch rename to tree_tracking/launch/tree_detector.launch diff --git a/hokuyo_estimation/launch/tree_detector_visualize.launch b/tree_tracking/launch/tree_detector_visualize.launch similarity index 100% rename from hokuyo_estimation/launch/tree_detector_visualize.launch rename to tree_tracking/launch/tree_detector_visualize.launch diff --git a/hokuyo_estimation/launch/urg_node.launch b/tree_tracking/launch/urg_node.launch similarity index 100% rename from hokuyo_estimation/launch/urg_node.launch rename to tree_tracking/launch/urg_node.launch diff --git a/hokuyo_estimation/package.xml b/tree_tracking/package.xml similarity index 96% rename from hokuyo_estimation/package.xml rename to tree_tracking/package.xml index d9683b016..9ced8961e 100644 --- a/hokuyo_estimation/package.xml +++ b/tree_tracking/package.xml @@ -1,8 +1,8 @@ - hokuyo_estimation + tree_tracking 0.0.0 - The hokuyo_estimation package + The tree tracking package Haruki Kozuka diff --git a/hokuyo_estimation/param/LaserClustering.yaml b/tree_tracking/param/LaserClustering.yaml similarity index 100% rename from hokuyo_estimation/param/LaserClustering.yaml rename to tree_tracking/param/LaserClustering.yaml diff --git a/hokuyo_estimation/param/LaserShadowFilter.yaml b/tree_tracking/param/LaserShadowFilter.yaml similarity index 100% rename from hokuyo_estimation/param/LaserShadowFilter.yaml rename to tree_tracking/param/LaserShadowFilter.yaml diff --git a/hokuyo_estimation/src/circle_detection.cpp b/tree_tracking/src/circle_detection.cpp similarity index 100% rename from hokuyo_estimation/src/circle_detection.cpp rename to tree_tracking/src/circle_detection.cpp diff --git a/hokuyo_estimation/src/laser_clustering_filter.cpp b/tree_tracking/src/laser_clustering_filter.cpp similarity index 100% rename from hokuyo_estimation/src/laser_clustering_filter.cpp rename to tree_tracking/src/laser_clustering_filter.cpp diff --git a/hokuyo_estimation/src/tree_database.cpp b/tree_tracking/src/tree_database.cpp similarity index 100% rename from hokuyo_estimation/src/tree_database.cpp rename to tree_tracking/src/tree_database.cpp diff --git a/hokuyo_estimation/src/tree_tracking.cpp b/tree_tracking/src/tree_tracking.cpp similarity index 100% rename from hokuyo_estimation/src/tree_tracking.cpp rename to tree_tracking/src/tree_tracking.cpp diff --git a/hokuyo_estimation/src/tree_tracking_node.cpp b/tree_tracking/src/tree_tracking_node.cpp similarity index 100% rename from hokuyo_estimation/src/tree_tracking_node.cpp rename to tree_tracking/src/tree_tracking_node.cpp From aceeba84afc045eb58ecf68fc82b655f976418b0 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 22:16:56 +0900 Subject: [PATCH 30/40] [hokuyo_estimation] rename tree_tracking include dir --- tree_tracking/src/circle_detection.cpp | 2 +- tree_tracking/src/laser_clustering_filter.cpp | 2 +- tree_tracking/src/tree_database.cpp | 2 +- tree_tracking/src/tree_tracking.cpp | 4 ++-- tree_tracking/src/tree_tracking_node.cpp | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/tree_tracking/src/circle_detection.cpp b/tree_tracking/src/circle_detection.cpp index 59462ab22..c39399031 100644 --- a/tree_tracking/src/circle_detection.cpp +++ b/tree_tracking/src/circle_detection.cpp @@ -1,4 +1,4 @@ -#include "circle_detection.h" +#include "tree_tracking/circle_detection.h" namespace CircleDetection { void circleFitting(const std::vector& points, tf::Vector3& tree_center_location, double& tree_radius, double& regulation) diff --git a/tree_tracking/src/laser_clustering_filter.cpp b/tree_tracking/src/laser_clustering_filter.cpp index 9fb247ec0..ab3d26401 100644 --- a/tree_tracking/src/laser_clustering_filter.cpp +++ b/tree_tracking/src/laser_clustering_filter.cpp @@ -27,7 +27,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "laser_clustering_filter.h" +#include "tree_tracking/laser_clustering_filter.h" #include "sensor_msgs/LaserScan.h" #include "filters/filter_base.h" diff --git a/tree_tracking/src/tree_database.cpp b/tree_tracking/src/tree_database.cpp index 6369ee204..044458682 100644 --- a/tree_tracking/src/tree_database.cpp +++ b/tree_tracking/src/tree_database.cpp @@ -1,4 +1,4 @@ -#include "tree_database.h" +#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) { diff --git a/tree_tracking/src/tree_tracking.cpp b/tree_tracking/src/tree_tracking.cpp index a04754f77..b0fccd1dc 100644 --- a/tree_tracking/src/tree_tracking.cpp +++ b/tree_tracking/src/tree_tracking.cpp @@ -1,5 +1,5 @@ -#include "tree_tracking.h" -#include "circle_detection.h" +#include "tree_tracking/tree_tracking.h" +#include "tree_tracking/circle_detection.h" TreeTracking::TreeTracking(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh), nhp_(nhp), diff --git a/tree_tracking/src/tree_tracking_node.cpp b/tree_tracking/src/tree_tracking_node.cpp index 78e26ec10..95ad17dc4 100644 --- a/tree_tracking/src/tree_tracking_node.cpp +++ b/tree_tracking/src/tree_tracking_node.cpp @@ -1,4 +1,4 @@ -#include "tree_tracking.h" +#include "tree_tracking/tree_tracking.h" int main (int argc, char **argv) { From 58f186b3fa2f1b4489c79d8c7fbda7264b177857 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 22:19:02 +0900 Subject: [PATCH 31/40] [tree_tracking] not change CMAKE_CXX_FLAGS but add compile option by add_compile_options --- tree_tracking/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tree_tracking/CMakeLists.txt b/tree_tracking/CMakeLists.txt index 53581e49b..94fe64f84 100644 --- a/tree_tracking/CMakeLists.txt +++ b/tree_tracking/CMakeLists.txt @@ -25,7 +25,8 @@ find_package(catkin REQUIRED COMPONENTS ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +# 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") From 35e55a78179e53f467404b2131cd3e88aa8cfe5f Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 22:28:32 +0900 Subject: [PATCH 32/40] [tree_tracking] add tree_tracking_node to targets --- tree_tracking/CMakeLists.txt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tree_tracking/CMakeLists.txt b/tree_tracking/CMakeLists.txt index 94fe64f84..88d5fa597 100644 --- a/tree_tracking/CMakeLists.txt +++ b/tree_tracking/CMakeLists.txt @@ -58,6 +58,11 @@ target_link_libraries (tree_tracking_node ${catkin_LIBRARIES} tree_tracking) ############# ## Install ## ############# -install(FILES laser_filters_plugins.xml +install( + TARGETS tree_tracking_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) +install( + FILES laser_filters_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) From 9e7b6a9c99fd8dadab8c980fba4c09bb2c9094f9 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 22:33:44 +0900 Subject: [PATCH 33/40] [tree_tracking] add rviz and urg_node in exec_depend --- tree_tracking/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tree_tracking/package.xml b/tree_tracking/package.xml index 9ced8961e..7a1acb837 100644 --- a/tree_tracking/package.xml +++ b/tree_tracking/package.xml @@ -51,6 +51,8 @@ tf laser_filters filters + rviz + urg_node From d3908a1772ecc552bbb9f378a02d3ba4829100e9 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 8 Jun 2024 22:41:31 +0900 Subject: [PATCH 34/40] [tree_tracking] install tree_tracking --- tree_tracking/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/tree_tracking/CMakeLists.txt b/tree_tracking/CMakeLists.txt index 88d5fa597..03067a08f 100644 --- a/tree_tracking/CMakeLists.txt +++ b/tree_tracking/CMakeLists.txt @@ -52,6 +52,12 @@ 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) From defbc37b84cfc4710d7c66942b099b16d8f8ce85 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 10 Jun 2024 17:38:43 +0900 Subject: [PATCH 35/40] [tree_estimation] set include variable before catkin_package --- tree_tracking/CMakeLists.txt | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/tree_tracking/CMakeLists.txt b/tree_tracking/CMakeLists.txt index 03067a08f..0d084db64 100644 --- a/tree_tracking/CMakeLists.txt +++ b/tree_tracking/CMakeLists.txt @@ -31,11 +31,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 -) +# catkin_package( +# INCLUDE_DIRS include +# CATKIN_DEPENDS roscpp sensor_msgs nodelet tf std_msgs dynamic_reconfigure geometry_msgs +# # LIBRARIES tree_tracking +# ) ########### ## Build ## @@ -46,6 +46,12 @@ include_directories( ${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}) From aa6755e4b636f76bc9b194b75aa3b8a52ed47177 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 10 Jun 2024 17:48:08 +0900 Subject: [PATCH 36/40] [tree_tracking][debug] delete catkin_package --- tree_tracking/CMakeLists.txt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tree_tracking/CMakeLists.txt b/tree_tracking/CMakeLists.txt index 0d084db64..194fdaf46 100644 --- a/tree_tracking/CMakeLists.txt +++ b/tree_tracking/CMakeLists.txt @@ -46,11 +46,11 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp sensor_msgs nodelet tf std_msgs dynamic_reconfigure geometry_msgs - # LIBRARIES tree_tracking -) +# 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}) From 7c2e7fa9fb487ecdb39e70bbbabeb1efef0ebda9 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 10 Jun 2024 17:51:43 +0900 Subject: [PATCH 37/40] [tree_tracking] include LIBRARIES --- tree_tracking/CMakeLists.txt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tree_tracking/CMakeLists.txt b/tree_tracking/CMakeLists.txt index 194fdaf46..8d0039c8a 100644 --- a/tree_tracking/CMakeLists.txt +++ b/tree_tracking/CMakeLists.txt @@ -31,11 +31,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 -# ) +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp sensor_msgs nodelet tf std_msgs dynamic_reconfigure geometry_msgs + LIBRARIES tree_tracking +) ########### ## Build ## From 3be92842655fe5a1fd9fc210c04f0a03993fa083 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 10 Jun 2024 18:08:29 +0900 Subject: [PATCH 38/40] [tree_tracking][debug] add test to inclde_directories --- tree_tracking/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tree_tracking/CMakeLists.txt b/tree_tracking/CMakeLists.txt index 8d0039c8a..8d4f13e08 100644 --- a/tree_tracking/CMakeLists.txt +++ b/tree_tracking/CMakeLists.txt @@ -41,7 +41,7 @@ catkin_package( ## Build ## ########### include_directories( - include + include test ${OpenCV_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) From 1112858783f45681366fe6c622f0e381c31b2bef Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 10 Jun 2024 18:13:11 +0900 Subject: [PATCH 39/40] [tree_tracking][debug] add test to inclde_directories --- tree_tracking/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tree_tracking/CMakeLists.txt b/tree_tracking/CMakeLists.txt index 8d4f13e08..8d0039c8a 100644 --- a/tree_tracking/CMakeLists.txt +++ b/tree_tracking/CMakeLists.txt @@ -41,7 +41,7 @@ catkin_package( ## Build ## ########### include_directories( - include test + include ${OpenCV_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) From 981e4f1fb936f8272c6a9bc3b42f39ac83c9dd93 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Mon, 10 Jun 2024 20:50:20 +0900 Subject: [PATCH 40/40] [tree_tracking] add install directory in install command --- tree_tracking/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/tree_tracking/CMakeLists.txt b/tree_tracking/CMakeLists.txt index 8d0039c8a..986e76ae1 100644 --- a/tree_tracking/CMakeLists.txt +++ b/tree_tracking/CMakeLists.txt @@ -70,6 +70,9 @@ 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}