diff --git a/.gitmodules b/.gitmodules index e206b5b..27318ac 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "vectornav"] path = vectornav url = https://github.com/open-rdc/vectornav +[submodule "zed-sdk"] + path = zed-sdk + url = https://github.com/open-rdc/zed-sdk.git diff --git a/gnssnav/include/gnssnav/follower_node.hpp b/gnssnav/include/gnssnav/follower_node.hpp index 21f3c3f..0e5029f 100644 --- a/gnssnav/include/gnssnav/follower_node.hpp +++ b/gnssnav/include/gnssnav/follower_node.hpp @@ -21,6 +21,19 @@ namespace gnssnav{ class Follower : public rclcpp::Node{ + + typedef struct{ + double x; + double y; + }Force; + + typedef struct{ + double x; + double y; + double distance; + double theta; + }Obstacle; + public: GNSSNAV_PUBLIC explicit Follower(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); @@ -61,11 +74,14 @@ class Follower : public rclcpp::Node{ void setBasePose(void); double calculateYawFromQuaternion(const geometry_msgs::msg::Quaternion&); double calculateCrossError(); + // double ObstaclePotential(); std::pair convertECEFtoUTM(double x, double y, double z); controller::PositionPid pid; + Obstacle obstacle; + const int freq_ms; const int is_debug; const double ld_min_; @@ -96,6 +112,12 @@ class Follower : public rclcpp::Node{ double vectornav_base_y_=0; double vectornav_base_yaw_=0; +// + double th = 3.0; + double k_o = 1.0; +// + + PJ_CONTEXT *C; PJ *P; }; diff --git a/gnssnav/src/follower_node.cpp b/gnssnav/src/follower_node.cpp index 638439b..d9f64b6 100644 --- a/gnssnav/src/follower_node.cpp +++ b/gnssnav/src/follower_node.cpp @@ -166,6 +166,38 @@ double Follower::findLookaheadDistance(){ } } +// double Follower::ObstaclePotential() +// { +// Force force; + +// double dx = point_[idx_].pose.position.x - obstacle.x; +// double dy = point_[idx_].pose.position.y - obstacle.y; + +// obstacle.distance = std::hypot(dx, dy); +// obstacle.theta = std::atan2(dy, dx); +// // k, ld_ ,th のパラメータ設定 +// // distanceはlookaheadのdistance +// // obstacleのthetaとdistanceがほしい lookaheadとの角度がtheta +// // double force_r.x = -k_r*(ld_ - distance_)*std::cos(theta); +// // double force_r.y = -k_r*(ld_ - distance_)*std::sin(theta); + +// force.x = -k_o*(th - obstacle.distance)*std::cos(obstacle.theta); +// force.y = -k_o*(th - obstacle.distance)*std::sin(obstacle.theta); + +// dx = point_[idx_].pose.position.x - current_position_x_ + force.x; +// dy = point_[idx_].pose.position.y - current_position_y_ + force.y; + +// double target_angle = std::atan2(dy, dx); + +// double angle = current_yaw_; +// angle = std::atan2(std::sin(angle), std::cos(angle)); + +// double theta = target_angle - angle; +// theta = std::atan2(std::sin(theta), std::cos(theta)); + +// return theta; +// } + double Follower::calculateCrossError(){ double dx = point_[idx_].pose.position.x - current_position_x_; double dy = point_[idx_].pose.position.y - current_position_y_; @@ -189,6 +221,7 @@ void Follower::followPath(){ publishLookahead(); // 目標地点との角度のズレ + // double theta = calculateCrossError(); double theta = calculateCrossError(); v_ = v_max_; diff --git a/imagenav/CMakeLists.txt b/imagenav/CMakeLists.txt new file mode 100644 index 0000000..3d6f371 --- /dev/null +++ b/imagenav/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(imagenav) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# auto find dependencies +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_library(imagenav + src/imagenav_node.cpp + src/line_detector.cpp + src/obstacle_detector.cpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_dependencies( + ament_cmake_auto +) + +target_link_libraries(${PROJECT_NAME} + # 他の依存ライブラリ +) + +ament_auto_package( + INSTALL_TO_SHARE +) \ No newline at end of file diff --git a/imagenav/include/imagenav/debug_camera.hpp b/imagenav/include/imagenav/debug_camera.hpp new file mode 100644 index 0000000..ff9d602 --- /dev/null +++ b/imagenav/include/imagenav/debug_camera.hpp @@ -0,0 +1,17 @@ +#include + +#include +#include + +namespace debug_camera{ +class DebugCamera : public rclcpp::Node{ +public: + explicit DebugCamera(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + explicit DebugCamera(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + ~DebugCamera(); +private: + + ObjectDetectionParameters detection_parameters; +}; +} diff --git a/imagenav/include/imagenav/imagenav_node.hpp b/imagenav/include/imagenav/imagenav_node.hpp new file mode 100644 index 0000000..ed2b639 --- /dev/null +++ b/imagenav/include/imagenav/imagenav_node.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "imagenav/obstacle_detector.hpp" +#include "imagenav/line_detector.hpp" + +#include "utilities/position_pid.hpp" + +#include "imagenav/visibility_control.h" + +namespace imagenav{ + +class ImageNav : public rclcpp::Node{ +public: + IMAGENAV_PUBLIC + explicit ImageNav(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + IMAGENAV_PUBLIC + explicit ImageNav(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + +private: + rclcpp::Subscription::SharedPtr autonomous_flag_subscriber_; + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Publisher::SharedPtr image_pub_; + rclcpp::Publisher::SharedPtr cmd_pub_; + + rclcpp::TimerBase::SharedPtr timer_; + + void autonomousFlagCallback(const std_msgs::msg::Bool::SharedPtr msg); + void ImageCallback(const sensor_msgs::msg::Image::SharedPtr msg); + void ImageNavigation(void); + + const int interval_ms; + const double linear_max_; + const double angular_max_; + const bool visualize_flag_; + bool autonomous_flag_=false; + + int left_line_x=0; + int right_line_x=0; + + std::vector center_points; + + controller::PositionPid pid; + + imagenav::LineDetector line; + // まだ未実装 + imagenav::ObstacleDetector obstacle; +}; + +} // namespace imagenav diff --git a/imagenav/include/imagenav/line_detector.hpp b/imagenav/include/imagenav/line_detector.hpp new file mode 100644 index 0000000..64b6dec --- /dev/null +++ b/imagenav/include/imagenav/line_detector.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +#include +#include + +namespace imagenav{ + +class LineDetector { + +public: + LineDetector(); + cv::Mat detectLine(const cv::Mat& cv_img); + std::vector estimateLinePosition(const cv::Mat& img); + std::vector SlideWindowMethod(const cv::Mat& img, const int start_x, const int line); + cv::Mat WindowVisualizar(cv::Mat& img, const std::vector& points, const int line, const cv::Scalar color); + cv::Mat PointVisualizar(cv::Mat& img, const std::vector& points); + +private: + cv::Mat draw_lines(const cv::Mat& cv_img, const std::vector& line); + std::vector Spline(const std::vector xs, const std::vector ys, double num_point); + cv::Mat toBEV(const cv::Mat& img); + bool JunctionDetectWindow(const cv::Mat& img, const cv::Point pos); + + std::vector prev_start_xs={100, 350}; + std::vector window_width={100, 100}; + + std::vector> prev_window_position = { + {cv::Point(0,0),cv::Point(0,0),cv::Point(0,0),cv::Point(0,0),cv::Point(0,0),cv::Point(0,0),cv::Point(0,0)}, + {cv::Point(0,0),cv::Point(0,0),cv::Point(0,0),cv::Point(0,0),cv::Point(0,0),cv::Point(0,0),cv::Point(0,0)} + }; +}; + +} // namespace \ No newline at end of file diff --git a/imagenav/include/imagenav/obstacle_detector.hpp b/imagenav/include/imagenav/obstacle_detector.hpp new file mode 100644 index 0000000..241ea6f --- /dev/null +++ b/imagenav/include/imagenav/obstacle_detector.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include + +namespace imagenav{ + +class ObstacleDetector { + +typedef struct{ + double x; + double y; +}Obstacle; + +public: + ObstacleDetector(); + std::vector detectObstacle(const cv::Mat& img); + +private: + cv::Mat MaskObstacleImage(const cv::Mat& img); + std::vector EstimatePosition(const cv::Mat& img); +}; + +} // namespace \ No newline at end of file diff --git a/imagenav/include/imagenav/visibility_control.h b/imagenav/include/imagenav/visibility_control.h new file mode 100644 index 0000000..852ba67 --- /dev/null +++ b/imagenav/include/imagenav/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef IMAGENAV__VISIBILITY_CONTROL_H_ +#define IMAGENAV__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define IMAGENAV_EXPORT __attribute__ ((dllexport)) + #define IMAGENAV_IMPORT __attribute__ ((dllimport)) + #else + #define IMAGENAV_EXPORT __declspec(dllexport) + #define IMAGENAV_IMPORT __declspec(dllimport) + #endif + #ifdef IMAGENAV_BUILDING_LIBRARY + #define IMAGENAV_PUBLIC IMAGENAV_EXPORT + #else + #define IMAGENAV_PUBLIC IMAGENAV_IMPORT + #endif + #define IMAGENAV_PUBLIC_TYPE IMAGENAV_PUBLIC + #define IMAGENAV_LOCAL +#else + #define IMAGENAV_EXPORT __attribute__ ((visibility("default"))) + #define IMAGENAV_IMPORT + #if __GNUC__ >= 4 + #define IMAGENAV_PUBLIC __attribute__ ((visibility("default"))) + #define IMAGENAV_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define IMAGENAV_PUBLIC + #define IMAGENAV_LOCAL + #endif + #define IMAGENAV_PUBLIC_TYPE +#endif + +#endif // IMAGENAV__VISIBILITY_CONTROL_H_ diff --git a/imagenav/package.xml b/imagenav/package.xml new file mode 100644 index 0000000..98a890e --- /dev/null +++ b/imagenav/package.xml @@ -0,0 +1,28 @@ + + + + imagenav + 0.0.0 + TODO: Package description + kyo + TODO: License declaration + + ament_cmake_ros + + rclcpp + std_msgs + sensor_msgs + nav_msgs + geometry_msgs + cv_bridge + image_transport + opencv2 + utilities + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/imagenav/src/debug_camera.cpp b/imagenav/src/debug_camera.cpp new file mode 100644 index 0000000..33bfb09 --- /dev/null +++ b/imagenav/src/debug_camera.cpp @@ -0,0 +1,69 @@ +#include "debug_camera/debug_camera.hpp" +#include "utilities/data_utils.hpp" + +namespace debug_Camera{ +DebugCamera::DebugCamera(const rclcpp::NodeOptions &options) : DebugCamera("", options) {} + +DebugCamera::DebugCamera(const std::string &name_space, const rclcpp::NodeOptions &options) +: rclcpp::Node("debug_camera_node", name_space, options) +{ + InitParameters init_params; + init_params.camera_resolution = RESOLUTION::SVGA; + init_params.camera_fps = 60; + + // 検出がframeごとに実行 + detection_parameters.image_sync = true; + // 同一IDを保持 + detection_parameters.enable_tracking = true; + // オブジェクト上に2Dマスクを作成 + detection_parameters.enable_mask_output = false; + + OpenCamera(); + + timer_ = this->create_wall_timer(std::chrono::milliseconds(100), + std::bind(&DebugCamera::DebugObjectDetection, this)); +} + +DebugCamera::~DebugCamera() +{ + zed.disableObjectDetection(); + zed.close(); + + cout << "camera closed" << endl; +} + +void DebugCamera::OpenCamera() +{ + err = zed.opend(init_params); + if(err != ERROR_CODE::SUCCESS) + exit(-1); +} + +void DebugCamera::DebugObjectDetection() +{ + ObjectDetectionRuntimeParameters detection_parameters_runtime; + detection_parameters_runtime.detection_confidence_threshold = 40; + + Object objects; + while(zed.grad() == ERROR_CODE::SUCESS) + { + err = zed.retrieveObjects(objects, detection_parameters_runtime); + + if(objects.is_new) + { + cout << objects.object_list.size() << "Object deteted" << endl; + + first_object = object.objet_list[0]; + cout << "first object position : " << first_object.position; + } + } +} + +} // namespace + +int main(int argc, char * argv[]){ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/imagenav/src/imagenav_node.cpp b/imagenav/src/imagenav_node.cpp new file mode 100644 index 0000000..a2a577b --- /dev/null +++ b/imagenav/src/imagenav_node.cpp @@ -0,0 +1,113 @@ +#include "imagenav/imagenav_node.hpp" + +#include "utilities/utils.hpp" +#include "cmath" + +namespace imagenav{ + +ImageNav::ImageNav(const rclcpp::NodeOptions &options) : ImageNav("", options) {} + +ImageNav::ImageNav(const std::string &name_space, const rclcpp::NodeOptions &options) +: rclcpp::Node("imagenav_node", name_space, options), +interval_ms(get_parameter("interval_ms").as_int()), +pid(get_parameter("interval_ms").as_int()), +linear_max_(get_parameter("max_linear_vel").as_double()), +angular_max_(get_parameter("max_angular_vel").as_double()), +visualize_flag_(get_parameter("visualize_flag").as_bool()) +{ + autonomous_flag_subscriber_ = this->create_subscription("/autonomous", 10, std::bind(&ImageNav::autonomousFlagCallback, this, std::placeholders::_1)); + image_sub_ = this->create_subscription("/zed/zed_node/rgb/image_rect_color", 10, std::bind(&ImageNav::ImageCallback, this, std::placeholders::_1)); + image_pub_ = this->create_publisher("/imagenav/image", 10); + cmd_pub_ = this->create_publisher("/cmd_vel", 10); + + timer_ = this->create_wall_timer(std::chrono::milliseconds(interval_ms), + std::bind(&ImageNav::ImageNavigation, this)); + + pid.gain(get_parameter("p_gain").as_double(), get_parameter("i_gain").as_double(), get_parameter("d_gain").as_double()); +} + +void ImageNav::autonomousFlagCallback(const std_msgs::msg::Bool::SharedPtr msg) +{ + autonomous_flag_ = msg->data; + RCLCPP_INFO(this->get_logger(), "Autonomous flag updated to: %s", autonomous_flag_ ? "true" : "false"); +} + +void ImageNav::ImageCallback(const sensor_msgs::msg::Image::SharedPtr img) +{ + cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img, img->encoding); + if(cv_img->image.empty() || !autonomous_flag_) return; + + cv::Mat image = cv_img->image; + cv::Mat detect_img = line.detectLine(cv_img->image); + std::vector obstacle_pos = obstacle.detectObstacle(image); + + // 白線の開始位置（x座標）を検出 + std::vector estimate_x = line.estimateLinePosition(detect_img); + + // スライドウィンドウ法で窓の中心点を抽出 + std::vector left_points = line.SlideWindowMethod(detect_img, estimate_x[0], 0); + std::vector right_points = line.SlideWindowMethod(detect_img, estimate_x[1], 1); + + for(size_t i=0; i < left_points.size(); ++i) + { + int x = (left_points[i].x + right_points[i].x) / 2; + int y = (left_points[i].y + right_points[i].y) / 2; + + if(obstacle_pos[0].x == -1){ + center_points.push_back(cv::Point(x, y)); + }else if(obstacle_pos[0].x <= x){ + + center_points.push_back(cv::Point((obstacle_pos[0].x + right_points[i].x*3) / 4, y)); + }else if(obstacle_pos[0].x >= x){ + center_points.push_back(cv::Point((left_points[i].x*3 + obstacle_pos[0].x) / 4, y)); + } + + } + + if(visualize_flag_) + { + cv::Mat point_img = line.WindowVisualizar(cv_img->image, left_points, 0, cv::Scalar(0,255,0)); + point_img = line.WindowVisualizar(point_img, right_points, 1, cv::Scalar(0,255,0)); + point_img = line.PointVisualizar(point_img, center_points); + + cv::line(point_img, cv::Point(obstacle_pos[0].x, 480), cv::Point(obstacle_pos[0].x, 0), cv::Scalar(255, 0, 0), 3); + + cv::imshow("point_img", point_img); + cv::waitKey(1); + + std_msgs::msg::Header header; + header.stamp = this->get_clock()->now(); + header.frame_id = "camera"; + + sensor_msgs::msg::Image::SharedPtr rosimg = cv_bridge::CvImage(header, "bgr8", point_img).toImageMsg(); + image_pub_->publish(*rosimg); + } +} + +void ImageNav::ImageNavigation(void) +{ + const int image_rows=240; + const int image_cols=240; + + if(center_points.empty() || !autonomous_flag_) + { + center_points.clear(); + return; + } + + // center_pointsに向かうように移動<-[4]は適当 + int dx = image_rows - center_points[3].x; + int dy = image_cols - center_points[3].y; + + center_points.clear(); + + double angle = std::atan2(dx, dy); // 座標変換のため, dx,dyを入れ替え + + geometry_msgs::msg::Twist cmd_vel; + cmd_vel.linear.x = linear_max_; + cmd_vel.angular.z = pid.cycle(angle); + + cmd_pub_->publish(cmd_vel); +} + +} // namespace imagenav diff --git a/imagenav/src/line_detector.cpp b/imagenav/src/line_detector.cpp new file mode 100644 index 0000000..359d356 --- /dev/null +++ b/imagenav/src/line_detector.cpp @@ -0,0 +1,318 @@ +#include "imagenav/line_detector.hpp" + +#include "cmath" + +namespace imagenav{ + +LineDetector::LineDetector() +{ +} + +cv::Mat LineDetector::detectLine(const cv::Mat& cv_img) +{ + cv::Mat bev_img, hsl_img, img, edges; + std::vector channel; + + // 鳥瞰図変換<-パラメータチューニングがすごく難しい + bev_img = toBEV(cv_img); + + cv::cvtColor(cv_img, hsl_img, cv::COLOR_BGR2HLS); + cv::split(hsl_img, channel); + img = channel[1]; + cv::GaussianBlur(img, img, cv::Size(5, 5), 0); + cv::Mat grad_x, grad_y; + cv::Mat abs_grad_x, abs_grad_y; + + // ソーベルフィルタ<-エッジの強調処理 + cv::Sobel(img, grad_x, CV_16S, 1, 0, 5); + cv::convertScaleAbs(grad_x, abs_grad_x); + cv::Sobel(img, grad_y, CV_16S, 0, 1, 5); + cv::convertScaleAbs(grad_y, abs_grad_y); + + cv::addWeighted(abs_grad_x, 0.5, abs_grad_y, 0.5, 0, edges); + + return edges; +} + +std::vector LineDetector::estimateLinePosition(const cv::Mat& img) +{ + const int window_height = 40; + const int threshold = 10; + + int max_white_x = 0; + int max_x = 0; + std::vector start_xs; + + for(int line=0; line < 2; line++) + { + std::vector white_pixel_counts(window_width[line], 0); + + for(int y = img.rows - window_height; y < img.rows; ++y) + { + const uchar *pLine = img.ptr(y); + for(int x = 0; x < window_width[line]; ++x) + { + int screen_x = prev_start_xs[line] - window_width[line]/2 + x; + + if(pLine[screen_x] > 128) + white_pixel_counts[x]++; + } + } + + for(int x = 0; x < window_width[line]; ++x) + { + if(white_pixel_counts[x] > white_pixel_counts[x - 1]) + { + max_white_x = prev_start_xs[line] - window_width[line]/2 + x; + max_x = x; + } + } + + start_xs.push_back(max_white_x); + + if(start_xs[line] >= img.cols){ + start_xs[line] = 400; + }else if(start_xs[line] <= 0){ + start_xs[line] = 100; + } + + if(abs(prev_start_xs[line] - start_xs[line]) > 10) + { + start_xs[line] = prev_start_xs[line]; + } + + // 白ピクセルが規定量を下回ったらウィンドウ拡大 + if(white_pixel_counts[max_x] <= threshold){ + if(window_width[line] <= 200) + window_width[line] += 3; + }else{ + window_width[line] = 50; + } + + prev_start_xs[line] = start_xs[line]; + + // 検出が被ったらウィンドウを初期位置にリセット + if(abs(start_xs[0] - start_xs[1]) <= 20){ + std::cerr << "window reset" << std::endl; + start_xs = {100, 400}; + } + } + + return start_xs; +} + +// スライドウィンドウ法 +std::vector LineDetector::SlideWindowMethod(const cv::Mat& img, const int start_x, const int line) +{ + const int window_width = 100; + const int window_height = 40; + const int threshold = window_width/2; + std::vector window_position; + std::vector white_pixel_counts(window_width, 0); + int estimate_x = start_x; + int prev_delta_x = 0; + + for(int height = img.rows - 1; height > window_height; height-=window_height) + { + std::fill(white_pixel_counts.begin(), white_pixel_counts.end(), 0); + + for(int y = height; y > height - window_height; --y) + { + const uchar *pLine = img.ptr(y); + for(int x = estimate_x-(window_width/2); x < estimate_x+(window_width/2); ++x) + { + if(pLine[x] > 128) + { + int relative_x = x - (estimate_x - (window_width/2)); + white_pixel_counts[relative_x]++; + } + } + } + + // ウィンドウ内の白ピクセル中央値を計算 + std::vector white_pixel_indices; + for (int x = 0; x < window_width; ++x) + { + if (white_pixel_counts[x] > 20) + { + for (int i = 0; i < white_pixel_counts[x]; ++i) + { + white_pixel_indices.push_back(x); + } + } + } + + int median_relative_x = 0; + if (!white_pixel_indices.empty()) + { + size_t n = white_pixel_indices.size() / 2; + std::nth_element(white_pixel_indices.begin(), white_pixel_indices.begin() + n, white_pixel_indices.end()); + median_relative_x = white_pixel_indices[n]; + + estimate_x = estimate_x - (window_width / 2) + median_relative_x; + }else{ + estimate_x = prev_window_position[line][window_position.size()].x + prev_delta_x; + } + + // 検出したウィンドウが前回の位置と離れすぎていたら前回の位置を保持 + if(abs(estimate_x - prev_window_position[line][window_position.size()].x) >= threshold && prev_window_position[line][window_position.size()].x != 0) + { + estimate_x = prev_window_position[line][window_position.size()].x; + // 一つ前のウィンドウと離れていたら前のウィンドウの位置を保持 + if(window_position.size() > 0 && abs(estimate_x - window_position[window_position.size()-1].x) > window_width/2) + { + estimate_x = window_position[window_position.size()-1].x; + } + } + + // 分岐路検出 + if(window_position.size() > 0 && abs(estimate_x - window_position[window_position.size()-1].x) > window_width/5) + { + if(JunctionDetectWindow(img, cv::Point{estimate_x, height})) + { + for(int y=height-window_height/2; y > window_height; y-=window_height) + { + window_position.push_back(cv::Point{estimate_x, y}); + } + + return window_position; + } + } + + prev_delta_x = estimate_x - prev_window_position[line][window_position.size()].x; + // スライドウィンドウの中心座標<-window_position + window_position.push_back(cv::Point{estimate_x, height - window_height/2}); + } + + for(int i=0; i window_position; + std::vector white_pixel_counts(window_width, 0); + + cv::Mat visualize_img = img; + for(int height=pos.y-window_height; height > window_height; height-=window_height) + { + std::fill(white_pixel_counts.begin(), white_pixel_counts.end(), 0); + + for(int y=height; y>height - window_height; y--) + { + const uchar *pLine = img.ptr(y); + for(int x=pos.x-(window_width/2); x<=pos.x+(window_width/2); x++) + { + if(pLine[x] > 128) + { + int relative_x = x - (pos.x - (window_width/2)); + white_pixel_counts[relative_x]++; + } + } + } + window_position.push_back(cv::Point{pos.x, height}); + } + + visualize_img = WindowVisualizar(visualize_img, window_position, 0, cv::Scalar(255, 0, 255)); + + for(int i=0; i= threshold) + return true; + } + + return false; +} + +cv::Mat LineDetector::toBEV(const cv::Mat& img) +{ + // 俯瞰画像の変換座標 480x300 + cv::Point2f src_points[4] = { {-400, 300}, {100, 175}, {380, 175}, {880, 300} }; + cv::Point2f dst_points[4] = { {0, 480}, {0, 0}, {480, 0}, {480, 480} }; + + // cv::Point2f src_points[4] = { {0, 300}, {200, 160}, {280, 160}, {480, 300} }; + // cv::Point2f dst_points[4] = { {0, 480}, {0, 0}, {480, 0}, {480, 480} }; + + cv::Mat trans_mat = cv::getPerspectiveTransform(src_points, dst_points); + + cv::warpPerspective(img, img, trans_mat, img.size()); + + return img; +} + +cv::Mat LineDetector::WindowVisualizar(cv::Mat& img, const std::vector& points, const int line, const cv::Scalar color) +{ + const int BOX_SIZE_height = 40; + + for (const auto& point : points) + { + cv::Point top_left(point.x - window_width[line] / 2, point.y - BOX_SIZE_height / 2); + cv::Point bottom_right(point.x + window_width[line] / 2, point.y + BOX_SIZE_height / 2); + + // 四角形の描画 + cv::rectangle(img, top_left, bottom_right, color, 2); + } + + return img; +} + +cv::Mat LineDetector::PointVisualizar(cv::Mat& img, const std::vector& points) +{ + for(const auto& point : points) + { + cv::circle(img, point, 5, cv::Scalar(0, 0, 255), 3); + } + + return img; +} + +cv::Mat LineDetector::draw_lines(const cv::Mat& cv_img ,const std::vector& line) +{ + const int length = 100; + const int thickness = 3; + + for(size_t i = 0; i < line.size(); i++) + { + if(line[i][0] != line[i][2]) + { + double slope = (line[i][3] - line[i][1])/(line[i][2] - line[i][0]); + // 傾きから角度を求める + double theta = std::atan(slope); + if(slope > 0) + { + if(line[i][2] > line[i][0]) + { + int x = line[i][2] + length*std::cos(theta); + int y = line[i][3] + length*std::sin(theta); + cv::line(cv_img, cv::Point(x, y), cv::Point(line[i][0], line[i][1]), cv::Scalar(0, 0, 255), thickness); + }else + { + int x = line[i][0] + length*std::cos(theta); + int y = line[i][1] + length*std::sin(theta); + cv::line(cv_img, cv::Point(x, y), cv::Point(line[i][2], line[i][3]), cv::Scalar(0, 0, 255), thickness); + } + }else{ + if(line[i][2] > line[i][0]) + { + int x = line[i][0] + length*std::cos(theta); + int y = line[i][1] + length*std::sin(theta); + cv::line(cv_img, cv::Point(x, y), cv::Point(line[i][2], line[i][3]), cv::Scalar(0, 0, 255), thickness); + }else{ + int x = line[i][2] + length*std::cos(theta); + int y = line[i][3] + length*std::sin(theta); + cv::line(cv_img, cv::Point(x, y), cv::Point(line[i][0], line[i][1]), cv::Scalar(0, 0, 255), thickness); + } + } + } + } + return cv_img; +} + +} // namespace \ No newline at end of file diff --git a/imagenav/src/obstacle_detector.cpp b/imagenav/src/obstacle_detector.cpp new file mode 100644 index 0000000..2c4cbb6 --- /dev/null +++ b/imagenav/src/obstacle_detector.cpp @@ -0,0 +1,76 @@ +#include "imagenav/obstacle_detector.hpp" + +namespace imagenav{ + +ObstacleDetector::ObstacleDetector() +{ +} + +// 障害物の座標推定 +std::vector ObstacleDetector::detectObstacle(const cv::Mat& img) +{ + cv::Mat img_yuv; + cv::cvtColor(img, img_yuv, cv::COLOR_BGR2YUV); + + cv::Ptr clahe = cv::createCLAHE(2.0, cv::Size(8, 8)); + std::vector channels; + cv::split(img_yuv, channels); + // 輝度のチャンネルのみヒストグラム平滑化<-画像のコンストラストを改善 + clahe->apply(channels[0], channels[0]); + cv::merge(channels, img_yuv); + // gbrに戻す + cv::Mat img_gbr, img_hsv; + cv::cvtColor(img_yuv, img_gbr, cv::COLOR_YUV2BGR); + // hsvに変換 + cv::cvtColor(img_gbr, img_hsv, cv::COLOR_BGR2HSV); + + cv::Mat mask_img = MaskObstacleImage(img_hsv); + + return EstimatePosition(mask_img); +} + +cv::Mat ObstacleDetector::MaskObstacleImage(const cv::Mat& img) +{ + // 検出対象の色範囲を指定 + cv::Scalar LOW_COLOR1(0, 50, 50), HIGH_COLOR1(6, 255, 255); + cv::Scalar LOW_COLOR2(174, 50, 50), HIGH_COLOR2(180, 255, 255); + + cv::Mat mask1, mask2, mask; + cv::inRange(img, LOW_COLOR1, HIGH_COLOR1, mask1); + // cv::inRange(img, LOW_COLOR2, HIGH_COLOR2, mask2); + // cv::bitwise_and(mask1, mask2, mask); + + return mask1; +} + +std::vector ObstacleDetector::EstimatePosition(const cv::Mat& img) +{ + std::vector obstacles_x; + int max_white_x=-1; + + std::vector white_pixel_counts(img.cols, 0); + for(int y = 0; y < img.rows; ++y) + { + const uchar *pLine = img.ptr(y); + for(int x = 0; x < img.cols; ++x) + { + if(pLine[x] > 128) + { + white_pixel_counts[x]++; + } + } + } + + for(int x = 0; x < img.cols; ++x) + { + if(white_pixel_counts[x] > white_pixel_counts[x - 1] && white_pixel_counts[x] >= 10) + { + max_white_x = x; + } + } + + obstacles_x.push_back(cv::Point{max_white_x, 0}); + return obstacles_x; +} + +} // namespace \ No newline at end of file diff --git a/main_executor/CMakeLists.txt b/main_executor/CMakeLists.txt index c650fe5..0817282 100644 --- a/main_executor/CMakeLists.txt +++ b/main_executor/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(controller REQUIRED) find_package(chassis_driver REQUIRED) find_package(cybergear_interface REQUIRED) find_package(gnssnav REQUIRED) +find_package(imagenav REQUIRED) add_executable(main_exec src/main.cpp @@ -24,6 +25,7 @@ ament_target_dependencies(main_exec chassis_driver cybergear_interface gnssnav + imagenav ) target_include_directories(main_exec diff --git a/main_executor/config/main_params.yaml b/main_executor/config/main_params.yaml index 4a34f93..8683b57 100644 --- a/main_executor/config/main_params.yaml +++ b/main_executor/config/main_params.yaml @@ -2,7 +2,7 @@ launch: #起動パラメータ ros__parameters: can: true joy: true - vectornav : true + vectornav : false log_level : 10 /**: #ワイルドカード(ここのパラメータは全ノードから読める: <名前に注意>) @@ -72,4 +72,14 @@ gnssnav_follower_node: min_lookahead_distance : 3.0 max_linear_vel : 5.0 max_angular_vel : 3.14 - laps : 3 + laps : 0 + +imagenav_node: + ros__parameters: + interval_ms : 50 + max_linear_vel : 3.0 + max_angular_vel : 15.5 + visualize_flag : false + p_gain : 1.0 + i_gain : 0.0 + d_gain : 0.0 diff --git a/main_executor/launch/main_exec.launch.py b/main_executor/launch/main_exec.launch.py index 4bcf4b8..d53e108 100755 --- a/main_executor/launch/main_exec.launch.py +++ b/main_executor/launch/main_exec.launch.py @@ -68,6 +68,13 @@ def generate_launch_description(): 'vectornav.launch.py']) ) + # # zed-ros2-wrapper起動の作成 + # zed_wrapper_launch = launch.actions.IncludeLaunchDescription( + # PythonLaunchDescriptionSource([os.path.join( + # get_package_share_directory('zed-ros2-wraper'), 'launch'), + # 'zed_camera.launch.py']) + # ) + # 起動エンティティクラスの作成 launch_discription = LaunchDescription() @@ -78,6 +85,8 @@ def generate_launch_description(): launch_discription.add_entity(joy_node) if(launch_params['vectornav'] is True): launch_discription.add_action(vectornav_launch) + # if(launch_params['zed'] is True): + # launch_discription.add_action(zed_wrapper_launch) launch_discription.add_action(log_level_arg) launch_discription.add_action(sim_flag_arg) diff --git a/main_executor/package.xml b/main_executor/package.xml index 4491e9b..170679a 100644 --- a/main_executor/package.xml +++ b/main_executor/package.xml @@ -16,6 +16,7 @@ chassis_driver cybergear_interface gnssnav + imagenav ament_cmake diff --git a/main_executor/src/main.cpp b/main_executor/src/main.cpp index 7441154..66d17eb 100644 --- a/main_executor/src/main.cpp +++ b/main_executor/src/main.cpp @@ -6,6 +6,7 @@ #include "cybergear_interface/cybergear_interface_node.hpp" #include "gnssnav/path_publisher_node.hpp" #include "gnssnav/follower_node.hpp" +#include "imagenav/imagenav_node.hpp" int main(int argc, char * argv[]){ rclcpp::init(argc,argv); @@ -30,6 +31,7 @@ int main(int argc, char * argv[]){ auto cybergear_interface_node = std::make_shared(nodes_option); auto path_publisher_node = std::make_shared(nodes_option); auto follower_node = std::make_shared(nodes_option); + auto imagenav_node = std::make_shared(nodes_option); if(sim_flag){} if(not sim_flag){ @@ -40,7 +42,8 @@ int main(int argc, char * argv[]){ exec.add_node(chassis_driver_node); exec.add_node(cybergear_interface_node); exec.add_node(path_publisher_node); - exec.add_node(follower_node); + // exec.add_node(follower_node); + exec.add_node(imagenav_node); exec.spin(); rclcpp::shutdown(); diff --git a/simulator/simulator/models/ai_car1/model.sdf b/simulator/simulator/models/ai_car1/model.sdf index a4388c9..873c2cf 100644 --- a/simulator/simulator/models/ai_car1/model.sdf +++ b/simulator/simulator/models/ai_car1/model.sdf @@ -240,28 +240,55 @@ - - base_link - camera_link - 0.35 0 1.5 0 0.087 0 - - - 0 0 1 - - - - - camera_link - camera_rgb_frame - 0.35 0 1.5 0 0.087 0 - - - 0 0 1 - - - - - + + base_link + camera_link + 0.35 0 1.5 0 0.087 0 + + + 0 0 1 + + + + + camera_link + camera_rgb_frame + 0.35 0 1.5 0 0.087 0 + + + 0 0 1 + + + + + + 0.35 0 1.5 0 0.087 0 @@ -276,18 +303,17 @@ 0.035 - 0.35 0 1.5 0 0.087 0 true true - 30 + 60 1.02974 - 1920 - 1080 + 480 + 300 R8G8B8 @@ -306,6 +332,8 @@ + /camera/image_raw:=/zed/zed_node/rgb/image_rect_color + /camera/camera_info:=/zed/zed_node/rgb/camera_info diff --git a/zed-sdk b/zed-sdk new file mode 160000 index 0000000..3472a79 --- /dev/null +++ b/zed-sdk @@ -0,0 +1 @@ +Subproject commit 3472a79fc635a9cee048e9c3e960cc48348415f0