Create b obstacle expander and b global path planner

b_global_path_planner/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.5)
+project(b_global_path_planner)
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+# package.xmlのdependをチェックして自動的にfind_packageしてくれる
+ament_auto_find_build_dependencies()
+# 実行ファイルを作成する
+ament_auto_add_executable(${PROJECT_NAME}_node
+  src/b_global_path_planner.cpp
+  src/b_global_path_planner_node.cpp)
+# launchファイルをインストール（ament_package_indexが探せる場所に置く）
+install(DIRECTORY
+  DESTINATION share/${PROJECT_NAME}/
+)
+# ヘッダーのインストールや共有ライブラリのエクスポート等を自動で行ってくれる
+ament_auto_package()
+

b_global_path_planner/config/param/global_path_planner_with_debag.yaml

@@ -0,0 +1,13 @@
+# 周期
+hz: 10
+
+# デバッグ用
+test_show: true
+sleep_time: 500000000
+
+# 経由点
+way_points_x: [0.0, 9.81, 16.3,16.52, 16.58, 10.45, 0.0, -7.65, -17.5, -17.4, -17.19, -9.36,0.0]
+way_points_y: [0.0, 0.0, 0.0,7.31, 14.38, 14.40, 14.16, 13.92, 13.76, 6.78, 0.0, 0.0,0.0]
+
+# 車両マージン
+margin: 0.25

b_global_path_planner/config/rviz/global_path_planner_with_debag.rviz

@@ -0,0 +1,199 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map2/Status1
+      Splitter Ratio: 0.5
+    Tree Height: 410
+  - 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: ""
+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: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 170; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /current_path
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /global_path
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/PointStamped
+      Color: 204; 41; 204
+      Enabled: true
+      History Length: 1
+      Name: PointStamped
+      Queue Size: 10
+      Radius: 0.20000000298023224
+      Topic: /current_node
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map/new_map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    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: 38.351600646972656
+      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: 14.211013793945312
+        Y: 3.169574499130249
+        Z: 1.8737245798110962
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.1947964429855347
+      Target Frame: <Fixed Frame>
+      Yaw: 3.095396041870117
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 701
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000223fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000223000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000223fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000223000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030700fffffffb0000000800540069006d006501000000000000045000000000000000000000023f0000022300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 60
+  Y: 60

b_global_path_planner/include/b_global_path_planner/b_global_path_planner.hpp

@@ -0,0 +1,104 @@
+#ifndef GLOBAL_PATH_PLANNER_H
+#define GLOBAL_PATH_PLANNER_H
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <vector>
+#include <math.h>
+
+class Astar: public rclcpp::Node
+{
+
+public:
+    Astar();
+    void process();
+    int hz_;  //周波数
+private:
+    struct Node
+    {
+        int    x = 0;
+        int    y = 0;
+        int    parent_x = - 1;
+        int    parent_y = -1;
+        double f = 0.0;
+    };
+
+    struct Motion
+    {
+        int dx;
+        int dy;
+        double cost;
+    };
+    void map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr mag);  //マップの読み込み
+    void obs_expander();
+    void obs_expand(const int index);
+    void swap_node(const Node node, std::vector<Node>& list1,std::vector<Node>& list2);  //リスト間の移動
+    void show_node_point(const Node node);  //ノードの表示
+    void show_path(nav_msgs::msg::Path& current_path);  //パスの表示
+    // void get_way_points(std::vector<std::vector<int>>& list);  //経由点の宣言
+    void planning();  //経路計画
+    void create_path(Node node);  //経路作成
+    void update_list(const Node node);  //隣接ノードの更新
+    void create_neighbors(const Node node, std::vector<Node>& nodes);  //隣接ノードの作成
+    void get_motion(std::vector<Motion>& motion);  //動作の定義
+    void show_exe_time();
+
+    bool check_same_node(const Node n1, const Node n2);  //同じノードかどうかの確認
+    bool check_obs(const Node node);  //壁か判断
+    bool check_start(const Node node);  //スタートかどうか
+    bool check_goal(const Node node);  //ゴールかどうか
+    bool check_parent(const int index, const Node node);
+
+    double make_heuristic(const Node node);  //ヒューリスティック関数の取得
+    int sleep_time_;
+    double margin_;
+    int check_list(const Node target_node, std::vector<Node>& set);  //リストの中を検索
+    int search_node_from_list(const Node node, std::vector<Node>& list);  //リストの中を検索
+    Node set_way_point(const int phase);  //経由点の取得
+    Node select_min_f();  //一番f値が小さいものを選択
+    Node get_neighbor(const Node node, const Motion motion);  //隣接ノードの取得
+    Motion motion(const int dx,const int dy,const int cost);  //もーしょん
+    geometry_msgs::msg::PoseStamped node_to_pose(const Node node);  //ノードから座標に変換
+
+    std::tuple<int, int> search_node(const Node node);  //どっちのリストに入っているのか検索
+
+    double origin_x_;  //原点のx座標
+    double origin_y_;  //原点のy座標
+    std::vector<double> way_points_x_;  //経由点
+    std::vector<double> way_points_y_;  //経由点
+
+
+    Node start_node_;  //開始位置
+    Node goal_node_;  //目標位置
+    std::vector<Node> open_list_;  //opneリスト
+    std::vector<Node> close_list_;  //closeリスト
+
+    bool map_checker_ = false;  //正しくマップはとれたかな
+    bool test_show_ = true;  //デバッグしますか
+
+    //map製作用
+    int height_;  //マップの幅だか高さだか
+    int width_;  //マップの幅だか高さだか
+    double resolution_;  //マップの解像度
+    Node origin_;  //マップの原点
+    std::vector<std::vector<int>> map_grid_;  //グリッドマップ
+
+    rclcpp::Clock begin_;
+
+    // rclcpp::NodeHandle nh_;
+    // rclcpp::NodeHandle private_nh_;
+    rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_map_;
+    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_path_;
+    rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr pub_node_point_;
+    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_current_path_;
+    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_new_map_;
+
+    nav_msgs::msg::Path global_path_;
+    nav_msgs::msg::OccupancyGrid map_;
+    nav_msgs::msg::OccupancyGrid new_map_;
+    geometry_msgs::msg::PointStamped current_node_;
+};
+#endif

b_global_path_planner/launch/global_path_planner_with_debag.launch

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- config file path  -->
+    <arg name="rviz_settings" default="$(find b_global_path_planner)/config/rviz/global_path_planner_with_debag.rviz"/>
+    <arg name="map_settings" default="$(find b_global_path_planner)/map/map.yaml"/>
+    <arg name="global_path_planner_settings" default="$(find b_global_path_planner)/config/param/global_path_planner_with_debag.yaml"/>
+
+    <!-- node launch -->
+    <node pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_settings)"/>
+    <node pkg="b_global_path_planner" type="b_global_path_planner_node" name="b_global_path_planner" output="screen">
+        <rosparam command="load" file="$(arg global_path_planner_settings)"/>
+    </node>
+    <node pkg="map_server" type="map_server" name="map_server" args="$(arg map_settings)" output="screen"/>
+</launch>

b_global_path_planner/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>b_global_path_planner</name>
+  <version>0.0.0</version>
+  <description>The b_global_path_planner package</description>
+  <maintainer email="amsl@todo.todo">amsl</maintainer>
+  <license>TODO</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>ament_cmake</depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>