diff --git a/b_global_path_planner/CMakeLists.txt b/b_global_path_planner/CMakeLists.txt new file mode 100644 index 0000000..e51fe20 --- /dev/null +++ b/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() + diff --git a/b_global_path_planner/config/param/global_path_planner_with_debag.yaml b/b_global_path_planner/config/param/global_path_planner_with_debag.yaml new file mode 100644 index 0000000..e308bed --- /dev/null +++ b/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 diff --git a/b_global_path_planner/config/rviz/global_path_planner_with_debag.rviz b/b_global_path_planner/config/rviz/global_path_planner_with_debag.rviz new file mode 100644 index 0000000..6716821 --- /dev/null +++ b/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: + 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: + 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 diff --git a/b_global_path_planner/include/b_global_path_planner/b_global_path_planner.hpp b/b_global_path_planner/include/b_global_path_planner/b_global_path_planner.hpp new file mode 100644 index 0000000..95bda8e --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include + +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& list1,std::vector& list2); //リスト間の移動 + void show_node_point(const Node node); //ノードの表示 + void show_path(nav_msgs::msg::Path& current_path); //パスの表示 + // void get_way_points(std::vector>& list); //経由点の宣言 + void planning(); //経路計画 + void create_path(Node node); //経路作成 + void update_list(const Node node); //隣接ノードの更新 + void create_neighbors(const Node node, std::vector& nodes); //隣接ノードの作成 + void get_motion(std::vector& 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& set); //リストの中を検索 + int search_node_from_list(const Node node, std::vector& 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 search_node(const Node node); //どっちのリストに入っているのか検索 + + double origin_x_; //原点のx座標 + double origin_y_; //原点のy座標 + std::vector way_points_x_; //経由点 + std::vector way_points_y_; //経由点 + + + Node start_node_; //開始位置 + Node goal_node_; //目標位置 + std::vector open_list_; //opneリスト + std::vector close_list_; //closeリスト + + bool map_checker_ = false; //正しくマップはとれたかな + bool test_show_ = true; //デバッグしますか + + //map製作用 + int height_; //マップの幅だか高さだか + int width_; //マップの幅だか高さだか + double resolution_; //マップの解像度 + Node origin_; //マップの原点 + std::vector> map_grid_; //グリッドマップ + + rclcpp::Clock begin_; + + // rclcpp::NodeHandle nh_; + // rclcpp::NodeHandle private_nh_; + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Publisher::SharedPtr pub_path_; + rclcpp::Publisher::SharedPtr pub_node_point_; + rclcpp::Publisher::SharedPtr pub_current_path_; + rclcpp::Publisher::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 diff --git a/b_global_path_planner/launch/global_path_planner_with_debag.launch b/b_global_path_planner/launch/global_path_planner_with_debag.launch new file mode 100644 index 0000000..9be74c2 --- /dev/null +++ b/b_global_path_planner/launch/global_path_planner_with_debag.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/map/map.pgm b/b_global_path_planner/map/map.pgm similarity index 77% rename from map/map.pgm rename to b_global_path_planner/map/map.pgm index e65989e..f94d41b 100644 --- a/map/map.pgm +++ b/b_global_path_planner/map/map.pgm @@ -1,4 +1,14 @@ P5 +# Created by GIMP version 2.10.36 PNM plug-in 1420 611 255 - \ No newline at end of file +5IejmmfR6kL+5!:G>;2-#-.&-('+8. &7=:+ +, &$& + (9FQWXSOLJKIGA<720+ɿ 4&Ѿ*&-!X((1 +J  9f#p(~19:2i  %s$   z+&xuhQj% :-! :-11@34466766788:I:K=J>M@QCF:K=MRW[`PcThYJ=1'jY=)    + !!! +  + + + +0Vxwrtrrokfddkd# 2x + ɼ0* \ No newline at end of file diff --git a/map/map.yaml b/b_global_path_planner/map/map.yaml similarity index 100% rename from map/map.yaml rename to b_global_path_planner/map/map.yaml diff --git a/b_global_path_planner/package.xml b/b_global_path_planner/package.xml new file mode 100644 index 0000000..5299a1f --- /dev/null +++ b/b_global_path_planner/package.xml @@ -0,0 +1,20 @@ + + + b_global_path_planner + 0.0.0 + The b_global_path_planner package + amsl + TODO + ament_cmake + + ament_cmake + rclcpp + std_msgs + geometry_msgs + nav_msgs + + + ament_cmake + + + diff --git a/b_global_path_planner/src/b_global_path_planner.cpp b/b_global_path_planner/src/b_global_path_planner.cpp new file mode 100644 index 0000000..a2874ea --- /dev/null +++ b/b_global_path_planner/src/b_global_path_planner.cpp @@ -0,0 +1,362 @@ +#include "b_global_path_planner/b_global_path_planner.hpp" + +Astar::Astar(): rclcpp::Node("b_global_path_planner") { + hz_ = this->declare_parameter("hz", 10); + test_show_ = this->declare_parameter("test_show", true); + way_points_x_ = this->declare_parameter>("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_ = this->declare_parameter>("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_ = this->declare_parameter("margin", 0.25); + sleep_time_ = this->declare_parameter("sleep_time", 500000000); + + sub_map_ = this->create_subscription( + "/map", rclcpp::QoS(1).reliable(), std::bind(&Astar::map_callback, this, std::placeholders::_1)); + + pub_path_ = this->create_publisher("/global_path", rclcpp::QoS(1)); + pub_new_map_ = this->create_publisher("/map/new_map", rclcpp::QoS(1)); + + if (test_show_) { + pub_current_path_ = this->create_publisher("/current_path", rclcpp::QoS(1)); + pub_node_point_ = this->create_publisher("/current_node", rclcpp::QoS(1)); + } + + global_path_.header.frame_id = "map"; + current_node_.header.frame_id = "map"; + + global_path_.poses.reserve(2000); +} + + +void Astar::map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + map_ = *msg; + origin_x_ = map_.info.origin.position.x; + origin_y_ = map_.info.origin.position.y; + height_ = map_.info.height; + width_ = map_.info.width; + resolution_ = map_.info.resolution; + map_checker_ = true; +} + + +void Astar::obs_expander() { + sleep(2); + new_map_ = map_; + int grid_size = new_map_.data.size(); + for (int i = 0; i < grid_size; i++) { + if (map_.data[i] == 100) { + obs_expand(i); + } + } + pub_new_map_->publish(new_map_); +} + +void Astar::obs_expand(const int index) { + int index_x = index % width_; + int index_y = index / width_; + int margin_length = round(margin_ / resolution_); + for (int i = -margin_length; i < margin_length; i++) { + for (int j = -margin_length; j < margin_length; j++) { + int grid_index = (index_x + j) + ((index_y + i) * width_); + new_map_.data[grid_index] = 100; + } + } +} + +Astar::Node Astar::set_way_point(const int phase) { + Node way_point; + way_point.x = round((way_points_x_[phase] - origin_x_) / resolution_); + way_point.y = round((way_points_y_[phase] - origin_y_) / resolution_); + return way_point; +} + +double Astar::make_heuristic(const Node node) { + const double dx = (node.x - goal_node_.x); + const double dy = (node.y - goal_node_.y); + const double distance = hypot(dx, dy); + return distance; +} + +bool Astar::check_same_node(const Node n1, const Node n2) { + return n1.x == n2.x and n1.y == n2.y; +} + +int Astar::check_list(const Node target_node, std::vector& set) { + for (int i = 0; i < set.size(); i++) { + if (check_same_node(target_node, set[i])) { + return i; + } + } + return -1; +} + +bool Astar::check_obs(const Node node) { + const int grid_index = node.x + (node.y * width_); + return new_map_.data[grid_index] == 100; +} + +void Astar::swap_node(const Node node, std::vector& list1, std::vector& list2) { + const int list1_node_index = check_list(node, list1); + if (list1_node_index == -1) { + exit(0); + } + + list1.erase(list1.begin() + list1_node_index); + list2.push_back(node); +} + +void Astar::show_node_point(const Node node) { + if (test_show_) { + current_node_.point.x = node.x * resolution_ + origin_x_; + current_node_.point.y = node.y * resolution_ + origin_y_; + pub_node_point_->publish(current_node_); + // node.cppで時間制御 + // rclcpp::Duration::from_nanoseconds(sleep_time_).sleep(); + } +} + +void Astar::show_path(nav_msgs::msg::Path& current_path) { + if (test_show_) { + current_path.header.frame_id = "map"; + pub_current_path_->publish(current_path); + // node.cpp で時間制御 + // rclcpp::Duration::from_nanoseconds(sleep_time_).sleep(); + } +} + +Astar::Node Astar::select_min_f() { + Node min_node = open_list_[0]; + double min_f = open_list_[0].f; + + for(const auto& open_node : open_list_) { + if(open_node.f < min_f) { + min_f = open_node.f; + min_node = open_node; + } + } + return min_node; +} + +bool Astar::check_goal(const Node node) { + return check_same_node(node,goal_node_); +} + +bool Astar::check_start(const Node node) { + return check_same_node(node,start_node_); +} + +void Astar::update_list(const Node node) { + std::vector neighbors; + create_neighbors(node, neighbors); + + for (const auto& neighbor : neighbors) { + if (check_obs(neighbor)) { + continue; + } + + int flag; + int node_index; + std::tie(flag, node_index) = search_node(neighbor); + + + if(flag == -1) + { + open_list_.push_back(neighbor); + } + else if(flag == 1) + { + if(neighbor.f < open_list_[node_index].f) + { + open_list_[node_index].f = neighbor.f; + open_list_[node_index].parent_x = neighbor.parent_x; + open_list_[node_index].parent_y = neighbor.parent_y; + } + + } + else if(flag == 2) + { + if(neighbor.f < close_list_[node_index].f) + { + close_list_.erase(close_list_.begin() + node_index); + open_list_.push_back(neighbor); + } + } + } +} + +void Astar::create_neighbors(const Node node, std::vector& neighbors) //隣接ノードの作成 +{ + std::vector motion_list; + get_motion(motion_list); + const int motion_num = motion_list.size(); + + for(int i=0;i& list) //動きの取得 +{ + list.push_back(motion(1,0,1)); + list.push_back(motion(0,1,1)); + list.push_back(motion(-1,0,1)); + list.push_back(motion(0,-1,1)); + + list.push_back(motion(1,1,sqrt(2))); + list.push_back(motion(1,-1,sqrt(2))); + list.push_back(motion(-1,1,sqrt(2))); + list.push_back(motion(-1,-1,sqrt(2))); +} + +Astar::Motion Astar::motion(const int dx,const int dy,const int cost) //もーしょん +{ + Motion motion; + motion.dx = dx; + motion.dy = dy; + motion.cost = cost; + + return motion; +} + +Astar::Node Astar::get_neighbor(const Node node, const Motion motion) //隣接ノードの取得 +{ + Node neighbor; + + neighbor.x = node.x + motion.dx; + neighbor.y = node.y + motion.dy; + + neighbor.f = (node.f - make_heuristic(node)) + make_heuristic(neighbor) + motion.cost; + + neighbor.parent_x = node.x; + neighbor.parent_y = node.y; + + return neighbor; +} + +std::tuple Astar::search_node(const Node node) //どこのリストに入っているかの検索 +{ + const int open_list_index = search_node_from_list(node,open_list_); + if(open_list_index != -1) + { + return std::make_tuple(1,open_list_index); + } + + const int close_list_index = search_node_from_list(node,close_list_); + if(close_list_index != -1) + { + return std::make_tuple(2,close_list_index); + } + return std::make_tuple(-1,-1); +} + +void Astar::create_path(Node node) //パスの作成 +{ + nav_msgs::msg::Path partial_path; + partial_path.poses.push_back(node_to_pose(node)); + int count = 0; + + while(not check_start(node)) + { + for(int i=0;i& list) //リストの中を検索 +{ + for(int i=0;ipublish(global_path_); + show_exe_time(); + // ROS_INFO("COMPLITE ASTAR PROGLAM"); + exit(0); +} + +void Astar::process() //メイン関数で実行する関数 +{ + + if (!map_checker_) + { + // ROS_INFO("NOW LOADING..."); + } + else + { + obs_expander(); + planning(); + } + +} diff --git a/b_global_path_planner/src/b_global_path_planner_node.cpp b/b_global_path_planner/src/b_global_path_planner_node.cpp new file mode 100644 index 0000000..df1f0f8 --- /dev/null +++ b/b_global_path_planner/src/b_global_path_planner_node.cpp @@ -0,0 +1,29 @@ +#include "b_global_path_planner/b_global_path_planner.hpp" + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + // FirstChallengeクラスをインスタンス化 + // ポインタを使う + std::shared_ptr astar_node = std::make_shared(); + + // 制御周期を定義 + rclcpp::Rate loop_rate(astar_node->hz_); + + // rclcpp::ok()によりCtrl+C押すまでプロセスを継続する + while (rclcpp::ok()) + { + // センサ情報を取得してから制御を開始 + // インスタンス化の際にポインタを指定したため，メンバ関数を指定する際にはアロー演算子（->）を使用 + astar_node->process(); + // コールバック関数を実行 + // rosのspinOnce + // 制御周期内で1度だけコールバック関数を実行する + rclcpp::spin_some(astar_node); + // 次の実行時間まで待つ + loop_rate.sleep(); + } + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file