✨feat: 白線認識

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<double, double> 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;
 };

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_;

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

imagenav/include/imagenav/debug_camera.hpp

@@ -0,0 +1,17 @@
+#include <iostream>
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/int64.hpp>
+
+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;
+};
+}

imagenav/include/imagenav/imagenav_node.hpp

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.hpp>
+#include <opencv2/opencv.hpp>
+
+#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<std_msgs::msg::Bool>::SharedPtr autonomous_flag_subscriber_;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::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<cv::Point> center_points;
+
+  controller::PositionPid pid;
+
+  imagenav::LineDetector line;
+  // まだ未実装
+  imagenav::ObstacleDetector obstacle;
+};
+
+}  // namespace imagenav

imagenav/include/imagenav/line_detector.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui.hpp>
+
+#include <image_transport/image_transport.hpp>
+#include <opencv2/opencv.hpp>
+
+namespace imagenav{
+
+class LineDetector {
+
+public:
+    LineDetector();
+    cv::Mat detectLine(const cv::Mat& cv_img);
+    std::vector<int> estimateLinePosition(const cv::Mat& img);
+    std::vector<cv::Point> SlideWindowMethod(const cv::Mat& img, const int start_x);
+    cv::Mat WindowVisualizar(cv::Mat& img, const std::vector<cv::Point>& points);
+    cv::Mat PointVisualizar(cv::Mat& img, const std::vector<cv::Point>& points);
+
+private:
+    cv::Mat draw_lines(const cv::Mat& cv_img, const std::vector<cv::Vec4i>& line);
+    std::vector<double> Spline(const std::vector<double> xs, const std::vector<double> ys, double num_point);
+    cv::Mat toBEV(const cv::Mat& img);
+    std::vector<int> prev_start_xs={100, 350};
+
+};
+
+} // namespace

imagenav/include/imagenav/obstacle_detector.hpp

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui.hpp>
+
+namespace imagenav{
+
+class ObstacleDetector {
+
+typedef struct{
+    double x;
+    double y;
+}Obstacle;
+
+public:
+    ObstacleDetector();
+    cv::Mat detectObstacle(const cv::Mat& img);
+
+private:
+    cv::Mat MaskObstacleImage(const cv::Mat& img);
+};
+
+} // namespace

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_

imagenav/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>imagenav</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">kyo</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>opencv2</depend>
+  <depend>utilities</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>