From eadffb83d6bb1a70998c51a0427ccf34ba0786b5 Mon Sep 17 00:00:00 2001 From: RikuHayakawa Date: Wed, 13 Mar 2024 15:40:36 +0900 Subject: [PATCH 1/8] add: b_global_path_planner_base --- b_global_path_planner/CMakeLists.txt | 18 + .../param/global_path_planner_with_debag.yaml | 13 + .../rviz/global_path_planner_with_debag.rviz | 199 +++++++++ .../b_global_path_planner.h | 103 +++++ .../global_path_planner_with_debag.launch | 14 + {map => b_global_path_planner/map}/map.pgm | 0 {map => b_global_path_planner/map}/map.yaml | 0 b_global_path_planner/package.xml | 20 + .../src/b_global_path_planner.cpp | 394 ++++++++++++++++++ .../src/b_global_path_planner_node.cpp | 10 + 10 files changed, 771 insertions(+) create mode 100644 b_global_path_planner/CMakeLists.txt create mode 100644 b_global_path_planner/config/param/global_path_planner_with_debag.yaml create mode 100644 b_global_path_planner/config/rviz/global_path_planner_with_debag.rviz create mode 100644 b_global_path_planner/include/b_global_path_planner/b_global_path_planner.h create mode 100644 b_global_path_planner/launch/global_path_planner_with_debag.launch rename {map => b_global_path_planner/map}/map.pgm (100%) rename {map => b_global_path_planner/map}/map.yaml (100%) create mode 100644 b_global_path_planner/package.xml create mode 100644 b_global_path_planner/src/b_global_path_planner.cpp create mode 100644 b_global_path_planner/src/b_global_path_planner_node.cpp 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..3e5ee8e --- /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: 0.005 + +# 経由点 +way_points_x: [0.0, 6.24, 11.92, 12.81, 13.23, 13.60, 10.75, 1.69, -7.27, -18.32, -20.14, -20.38, -20.50, -15.11, -6.95, 0.0] +way_points_y: [0.0, -0.38, -0.57, 2.36, 6.46, 12.54, 13.80, 14.11, 14.49, 15.14, 11.18, 7.24, 1.85, 0.68, 0.34, 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.h b/b_global_path_planner/include/b_global_path_planner/b_global_path_planner.h new file mode 100644 index 0000000..8e655b7 --- /dev/null +++ b/b_global_path_planner/include/b_global_path_planner/b_global_path_planner.h @@ -0,0 +1,103 @@ +#ifndef GLOBAL_PATH_PLANNER_H +#define GLOBAL_PATH_PLANNER_H + +#include +#include +#include +#include +#include +#include + +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; +}; + +class Astar +{ +public: + Astar(); + void process(); + +private: + void map_callback(const nav_msgs::OccupancyGrid::ConstPtr& 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::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); //ヒューリスティック関数の取得 + double 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::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_; //経由点 + + int hz_; //周波数 + Node start_node_; //開始位置 + Node goal_node_; //目標位置 + std::vector open_list_; //opneリスト + std::vector close_list_; //closeリスト + + bool map_checker_ = false; //正しくマップはとれたかな + bool test_show_; //デバッグしますか + + //map製作用 + int height_; //マップの幅だか高さだか + int width_; //マップの幅だか高さだか + double resolution_; //マップの解像度 + Node origin_; //マップの原点 + std::vector> map_grid_; //グリッドマップ + + ros::Time begin_; + + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + ros::Subscriber sub_map_; + ros::Publisher pub_path_; + ros::Publisher pub_node_point_; + ros::Publisher pub_current_path_; + ros::Publisher pub_new_map_; + + nav_msgs::Path global_path_; + nav_msgs::OccupancyGrid map_; + nav_msgs::OccupancyGrid new_map_; + geometry_msgs::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 100% rename from map/map.pgm rename to b_global_path_planner/map/map.pgm 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..1aeda84 --- /dev/null +++ b/b_global_path_planner/src/b_global_path_planner.cpp @@ -0,0 +1,394 @@ +#include "b_global_path_planner/b_global_path_planner.h" + +Astar::Astar():private_nh_("~") +{ + private_nh_.getParam("hz", hz_); + private_nh_.getParam("test_show", test_show_); + private_nh_.getParam("sleep_time", sleep_time_); + private_nh_.getParam("way_points_x", way_points_x_); + private_nh_.getParam("way_points_y", way_points_y_); + private_nh_.getParam("margin", margin_); + + global_path_.header.frame_id = "map"; + current_node_.header.frame_id = "map"; + + global_path_.poses.reserve(2000); + + sub_map_ = nh_.subscribe("/map", 1, &Astar::map_callback, this); + pub_path_ = nh_.advertise("/global_path", 1); + pub_new_map_ = nh_.advertise("/map/new_map",1); + if(test_show_) + { + pub_current_path_ = nh_.advertise("/current_path", 1); + pub_node_point_ = nh_.advertise("/current_node", 1); + } +} + + +void Astar::map_callback(const nav_msgs::OccupancyGrid::ConstPtr& 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() +{ + ROS_INFO("obs_expander is runnning..."); + sleep(2); + new_map_ = map_; + int grid_size = new_map_.data.size(); + for(int i=0;i& set) //リストの中を検索 +{ + for(int i=0;i& list1, std::vector& list2) //リスト間のノードの移動 +{ + const int list1_node_index = check_list(node,list1); + if(list1_node_index == -1) + { + ROS_INFO("swap node was faird"); + 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_); + ros::Duration(sleep_time_).sleep(); + } +} + +void Astar::show_path(nav_msgs::Path& current_path) //パスの表示 +{ + if(test_show_) + { + current_path.header.frame_id = "map"; + pub_current_path_.publish(current_path); + ros::Duration(sleep_time_).sleep(); + } +} + +Node Astar::select_min_f() //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))); +} + +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; +} + +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::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;i Date: Thu, 14 Mar 2024 17:30:23 +0900 Subject: [PATCH 2/8] =?UTF-8?q?change:=20ros=20=E2=86=92=20ros2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ...th_planner.h => b_global_path_planner.hpp} | 69 +++---- .../src/b_global_path_planner.cpp | 186 ++++++++---------- .../src/b_global_path_planner_node.cpp | 9 +- 3 files changed, 119 insertions(+), 145 deletions(-) rename b_global_path_planner/include/b_global_path_planner/{b_global_path_planner.h => b_global_path_planner.hpp} (64%) diff --git a/b_global_path_planner/include/b_global_path_planner/b_global_path_planner.h b/b_global_path_planner/include/b_global_path_planner/b_global_path_planner.hpp similarity index 64% rename from b_global_path_planner/include/b_global_path_planner/b_global_path_planner.h rename to b_global_path_planner/include/b_global_path_planner/b_global_path_planner.hpp index 8e655b7..a394f7f 100644 --- a/b_global_path_planner/include/b_global_path_planner/b_global_path_planner.h +++ b/b_global_path_planner/include/b_global_path_planner/b_global_path_planner.hpp @@ -1,42 +1,43 @@ #ifndef GLOBAL_PATH_PLANNER_H #define GLOBAL_PATH_PLANNER_H +#include #include -#include +#include #include #include #include #include -struct Node +class Astar: public rclcpp::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; -}; -class Astar -{ public: Astar(); void process(); private: - void map_callback(const nav_msgs::OccupancyGrid::ConstPtr& mag); //マップの読み込み + 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::Path& current_path); //パスの表示 + void show_path(nav_msgs::msg::Path& current_path); //パスの表示 // void get_way_points(std::vector>& list); //経由点の宣言 void planning(); //経路計画 void create_path(Node node); //経路作成 @@ -60,7 +61,7 @@ class Astar 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::PoseStamped node_to_pose(const Node node); //ノードから座標に変換 + geometry_msgs::msg::PoseStamped node_to_pose(const Node node); //ノードから座標に変換 std::tuple search_node(const Node node); //どっちのリストに入っているのか検索 @@ -75,8 +76,8 @@ class Astar std::vector open_list_; //opneリスト std::vector close_list_; //closeリスト - bool map_checker_ = false; //正しくマップはとれたかな - bool test_show_; //デバッグしますか + bool map_checker_ = false; //正しくマップはとれたかな + bool test_show_ = true; //デバッグしますか //map製作用 int height_; //マップの幅だか高さだか @@ -85,19 +86,19 @@ class Astar Node origin_; //マップの原点 std::vector> map_grid_; //グリッドマップ - ros::Time begin_; + rclcpp::Clock begin_; - ros::NodeHandle nh_; - ros::NodeHandle private_nh_; - ros::Subscriber sub_map_; - ros::Publisher pub_path_; - ros::Publisher pub_node_point_; - ros::Publisher pub_current_path_; - ros::Publisher pub_new_map_; + // 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::Path global_path_; - nav_msgs::OccupancyGrid map_; - nav_msgs::OccupancyGrid new_map_; - geometry_msgs::PointStamped current_node_; + 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/src/b_global_path_planner.cpp b/b_global_path_planner/src/b_global_path_planner.cpp index 1aeda84..d5db0d0 100644 --- a/b_global_path_planner/src/b_global_path_planner.cpp +++ b/b_global_path_planner/src/b_global_path_planner.cpp @@ -1,32 +1,32 @@ -#include "b_global_path_planner/b_global_path_planner.h" +#include "b_global_path_planner/b_global_path_planner.hpp" -Astar::Astar():private_nh_("~") -{ - private_nh_.getParam("hz", hz_); - private_nh_.getParam("test_show", test_show_); - private_nh_.getParam("sleep_time", sleep_time_); - private_nh_.getParam("way_points_x", way_points_x_); - private_nh_.getParam("way_points_y", way_points_y_); - private_nh_.getParam("margin", margin_); - - global_path_.header.frame_id = "map"; - current_node_.header.frame_id = "map"; +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, 6.24, 11.92, 12.81, 13.23, 13.60, 10.75, 1.69, -7.27, -18.32, -20.14, -20.38, -20.50, -15.11, -6.95, 0.0}); + way_points_y_ = this->declare_parameter>("way_points_y", {0.0, -0.38, -0.57, 2.36, 6.46, 12.54, 13.80, 14.11, 14.49, 15.14, 11.18, 7.24, 1.85, 0.68, 0.34, 0.0}); + margin_ = this->declare_parameter("margin", 0.25); + sleep_time_ = this->declare_parameter("sleep_time", 0.0005); - global_path_.poses.reserve(2000); + sub_map_ = this->create_subscription( + "/map", rclcpp::QoS(1).reliable(), std::bind(&Astar::map_callback, this, std::placeholders::_1)); - sub_map_ = nh_.subscribe("/map", 1, &Astar::map_callback, this); - pub_path_ = nh_.advertise("/global_path", 1); - pub_new_map_ = nh_.advertise("/map/new_map",1); - if(test_show_) - { - pub_current_path_ = nh_.advertise("/current_path", 1); - pub_node_point_ = nh_.advertise("/current_node", 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::OccupancyGrid::ConstPtr& msg) //マップの読み込み -{ +void Astar::map_callback(const nav_msgs::msg::OccupancyGrid& msg) { map_ = *msg; origin_x_ = map_.info.origin.position.x; origin_y_ = map_.info.origin.position.y; @@ -36,82 +36,66 @@ void Astar::map_callback(const nav_msgs::OccupancyGrid::ConstPtr& msg) //マッ map_checker_ = true; } -void Astar::obs_expander() -{ - ROS_INFO("obs_expander is runnning..."); + +void Astar::obs_expander() { sleep(2); new_map_ = map_; int grid_size = new_map_.data.size(); - for(int i=0;ipublish(new_map_); } -void Astar::obs_expand(const int index) -{ +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& set) //リストの中を検索 -{ - for(int i=0;i& 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) //壁の確認 -{ +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) - { - ROS_INFO("swap node was faird"); +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); } @@ -119,36 +103,29 @@ void Astar::swap_node(const Node node, std::vector& list1, std::vectorpublish(current_node_); + rclcpp::Duration(sleep_time_).sleep(); } } -void Astar::show_path(nav_msgs::Path& current_path) //パスの表示 -{ - if(test_show_) - { +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); - ros::Duration(sleep_time_).sleep(); + pub_current_path_->publish(current_path); + rclcpp::Duration(sleep_time_).sleep(); } } -Node Astar::select_min_f() //f値が小さいものを取得 -{ +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) - { + for(const auto& open_node : open_list_) { + if(open_node.f < min_f) { min_f = open_node.f; min_node = open_node; } @@ -156,27 +133,20 @@ Node Astar::select_min_f() //f値が小さいものを取得 return min_node; } -bool Astar::check_goal(const Node node) //あんたゴール？ -{ +bool Astar::check_goal(const Node node) { return check_same_node(node,goal_node_); } -bool Astar::check_start(const Node node) //あんたスタート？ -{ +bool Astar::check_start(const Node node) { return check_same_node(node,start_node_); } -void Astar::update_list(const Node node) //リストの更新 -{ +void Astar::update_list(const Node node) { std::vector neighbors; - create_neighbors(node, neighbors); - for(const auto& neighbor : neighbors) - { - - if(check_obs(neighbor)) - { + for (const auto& neighbor : neighbors) { + if (check_obs(neighbor)) { continue; } @@ -236,7 +206,7 @@ void Astar::get_motion(std::vector& list) //動きの取得 list.push_back(motion(-1,-1,sqrt(2))); } -Motion Astar::motion(const int dx,const int dy,const int cost) //もーしょん +Astar::Motion Astar::motion(const int dx,const int dy,const int cost) //もーしょん { Motion motion; motion.dx = dx; @@ -246,7 +216,7 @@ Motion Astar::motion(const int dx,const int dy,const int cost) //もーしょ return motion; } -Node Astar::get_neighbor(const Node node, const Motion motion) //隣接ノードの取得 +Astar::Node Astar::get_neighbor(const Node node, const Motion motion) //隣接ノードの取得 { Node neighbor; @@ -279,7 +249,7 @@ std::tuple Astar::search_node(const Node node) //どこのリスト void Astar::create_path(Node node) //パスの作成 { - nav_msgs::Path partial_path; + nav_msgs::msg::Path partial_path; partial_path.poses.push_back(node_to_pose(node)); int count = 0; @@ -296,7 +266,7 @@ void Astar::create_path(Node node) //パスの作成 } if(i == close_list_.size()-1) { - ROS_INFO("sippai......."); + // ROS_INFO("sippai......."); exit(0); } } @@ -314,9 +284,9 @@ bool Astar::check_parent(const int index, const Node node) //親ノードの確 return check_x and check_y; } -geometry_msgs::PoseStamped Astar::node_to_pose(const Node node) //ノードから座標の検索 +geometry_msgs::msg::PoseStamped Astar::node_to_pose(const Node node) //ノードから座標の検索 { - geometry_msgs::PoseStamped pose_stamped; + geometry_msgs::msg::PoseStamped pose_stamped; pose_stamped.pose.position.x = node.x * resolution_ + origin_x_; pose_stamped.pose.position.y = node.y * resolution_ + origin_y_; return pose_stamped; @@ -324,7 +294,7 @@ geometry_msgs::PoseStamped Astar::node_to_pose(const Node node) //ノードか void Astar::show_exe_time() { - ROS_INFO_STREAM("Duration = " << std::fixed << std::setprecision(2) << ros::Time::now().toSec() - begin_.toSec() << "s"); + // ROS_INFO_STREAM("Duration = " << std::fixed << std::setprecision(2) << rclcpp::Time::now().toSec() - begin_.toSec() << "s"); } int Astar::search_node_from_list(const Node node, std::vector& list) //リストの中を検索 @@ -339,7 +309,7 @@ int Astar::search_node_from_list(const Node node, std::vector& list) // void Astar::planning() //経路計画 { - begin_ = ros::Time::now(); + begin_ = rclcpp::Clock.now(); const int total_phase = way_points_x_.size(); for(int phase=0;phasepublish(global_path_); show_exe_time(); - ROS_INFO("COMPLITE ASTAR PROGLAM"); + // ROS_INFO("COMPLITE ASTAR PROGLAM"); exit(0); } void Astar::process() //メイン関数で実行する関数 { - ROS_INFO("process is starting..."); - ros::Rate loop_rate(hz_); + rclcpp::Node::SharedPtr node = std::make_shared("astar_node"); + rclcpp::Rate loop_rate(hz_); - while(ros::ok()) + while (rclcpp::ok()) { - if(!map_checker_) - ROS_INFO("NOW LOADING..."); + if (!map_checker_) + { + // ROS_INFO("NOW LOADING..."); + } else { obs_expander(); planning(); } - ros::spinOnce(); + rclcpp::spin_some(node); loop_rate.sleep(); } } 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 index fa03bef..8539b61 100644 --- a/b_global_path_planner/src/b_global_path_planner_node.cpp +++ b/b_global_path_planner/src/b_global_path_planner_node.cpp @@ -1,10 +1,11 @@ -#include "b_global_path_planner/b_global_path_planner.h" +#include "b_global_path_planner/b_global_path_planner.hpp" int main(int argc, char* argv[]) { - ros::init(argc, argv, "global_path_palnner"); - Astar astar; - astar.process(); + rclcpp::init(argc, argv); + auto astar_node = std::make_shared(); + astar_node->process(); + rclcpp::shutdown(); return 0; } \ No newline at end of file From bd8b2085aad6870f9a28e6d791e25060644c8aa8 Mon Sep 17 00:00:00 2001 From: RikuHayakawa Date: Thu, 14 Mar 2024 17:38:21 +0900 Subject: [PATCH 3/8] =?UTF-8?q?fix:=20=E6=99=82=E9=96=93=E5=88=B6=E5=BE=A1?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../param/global_path_planner_with_debag.yaml | 2 +- .../b_global_path_planner.hpp | 6 ++-- .../src/b_global_path_planner.cpp | 34 ++++++++----------- .../src/b_global_path_planner_node.cpp | 28 ++++++++++++--- 4 files changed, 42 insertions(+), 28 deletions(-) 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 index 3e5ee8e..8c4a8e5 100644 --- 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 @@ -3,7 +3,7 @@ hz: 10 # デバッグ用 test_show: true -sleep_time: 0.005 +sleep_time: 500000000 # 経由点 way_points_x: [0.0, 6.24, 11.92, 12.81, 13.23, 13.60, 10.75, 1.69, -7.27, -18.32, -20.14, -20.38, -20.50, -15.11, -6.95, 0.0] 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 index a394f7f..95bda8e 100644 --- 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 @@ -15,7 +15,7 @@ class Astar: public rclcpp::Node public: Astar(); void process(); - + int hz_; //周波数 private: struct Node { @@ -53,7 +53,7 @@ class Astar: public rclcpp::Node bool check_parent(const int index, const Node node); double make_heuristic(const Node node); //ヒューリスティック関数の取得 - double sleep_time_; + 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); //リストの中を検索 @@ -70,7 +70,7 @@ class Astar: public rclcpp::Node std::vector way_points_x_; //経由点 std::vector way_points_y_; //経由点 - int hz_; //周波数 + Node start_node_; //開始位置 Node goal_node_; //目標位置 std::vector open_list_; //opneリスト diff --git a/b_global_path_planner/src/b_global_path_planner.cpp b/b_global_path_planner/src/b_global_path_planner.cpp index d5db0d0..235fb7f 100644 --- a/b_global_path_planner/src/b_global_path_planner.cpp +++ b/b_global_path_planner/src/b_global_path_planner.cpp @@ -6,7 +6,7 @@ Astar::Astar(): rclcpp::Node("b_global_path_planner") { way_points_x_ = this->declare_parameter>("way_points_x", {0.0, 6.24, 11.92, 12.81, 13.23, 13.60, 10.75, 1.69, -7.27, -18.32, -20.14, -20.38, -20.50, -15.11, -6.95, 0.0}); way_points_y_ = this->declare_parameter>("way_points_y", {0.0, -0.38, -0.57, 2.36, 6.46, 12.54, 13.80, 14.11, 14.49, 15.14, 11.18, 7.24, 1.85, 0.68, 0.34, 0.0}); margin_ = this->declare_parameter("margin", 0.25); - sleep_time_ = this->declare_parameter("sleep_time", 0.0005); + 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)); @@ -26,7 +26,7 @@ Astar::Astar(): rclcpp::Node("b_global_path_planner") { } -void Astar::map_callback(const nav_msgs::msg::OccupancyGrid& msg) { +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; @@ -108,7 +108,8 @@ void Astar::show_node_point(const Node node) { current_node_.point.x = node.x * resolution_ + origin_x_; current_node_.point.y = node.y * resolution_ + origin_y_; pub_node_point_->publish(current_node_); - rclcpp::Duration(sleep_time_).sleep(); + // node.cppで時間制御 + // rclcpp::Duration::from_nanoseconds(sleep_time_).sleep(); } } @@ -116,7 +117,8 @@ 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); - rclcpp::Duration(sleep_time_).sleep(); + // node.cpp で時間制御 + // rclcpp::Duration::from_nanoseconds(sleep_time_).sleep(); } } @@ -309,7 +311,7 @@ int Astar::search_node_from_list(const Node node, std::vector& list) // void Astar::planning() //経路計画 { - begin_ = rclcpp::Clock.now(); + begin_ = rclcpp::Clock::now(); const int total_phase = way_points_x_.size(); for(int phase=0;phase("astar_node"); - rclcpp::Rate loop_rate(hz_); - while (rclcpp::ok()) + if (!map_checker_) { - if (!map_checker_) - { - // ROS_INFO("NOW LOADING..."); - } - else - { - obs_expander(); - planning(); - } - rclcpp::spin_some(node); - loop_rate.sleep(); + // 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 index 8539b61..df1f0f8 100644 --- a/b_global_path_planner/src/b_global_path_planner_node.cpp +++ b/b_global_path_planner/src/b_global_path_planner_node.cpp @@ -1,11 +1,29 @@ #include "b_global_path_planner/b_global_path_planner.hpp" -int main(int argc, char* argv[]) +int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - auto astar_node = std::make_shared(); + 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(); - rclcpp::shutdown(); + // コールバック関数を実行 + // rosのspinOnce + // 制御周期内で1度だけコールバック関数を実行する + rclcpp::spin_some(astar_node); + // 次の実行時間まで待つ + loop_rate.sleep(); + } + rclcpp::shutdown(); - return 0; + return 0; } \ No newline at end of file From 0ca611f5414f80b50b7c4947bf18aacca734a47b Mon Sep 17 00:00:00 2001 From: RikuHayakawa Date: Thu, 14 Mar 2024 17:47:50 +0900 Subject: [PATCH 4/8] fix: rclcpp::Clock --- b_global_path_planner/src/b_global_path_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/b_global_path_planner/src/b_global_path_planner.cpp b/b_global_path_planner/src/b_global_path_planner.cpp index 235fb7f..7895e72 100644 --- a/b_global_path_planner/src/b_global_path_planner.cpp +++ b/b_global_path_planner/src/b_global_path_planner.cpp @@ -311,7 +311,7 @@ int Astar::search_node_from_list(const Node node, std::vector& list) // void Astar::planning() //経路計画 { - begin_ = rclcpp::Clock::now(); + begin_.now(); const int total_phase = way_points_x_.size(); for(int phase=0;phase Date: Thu, 28 Mar 2024 15:06:57 +0900 Subject: [PATCH 5/8] wip --- b_global_path_planner/src/b_global_path_planner.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/b_global_path_planner/src/b_global_path_planner.cpp b/b_global_path_planner/src/b_global_path_planner.cpp index 7895e72..e5649f8 100644 --- a/b_global_path_planner/src/b_global_path_planner.cpp +++ b/b_global_path_planner/src/b_global_path_planner.cpp @@ -6,6 +6,7 @@ Astar::Astar(): rclcpp::Node("b_global_path_planner") { way_points_x_ = this->declare_parameter>("way_points_x", {0.0, 6.24, 11.92, 12.81, 13.23, 13.60, 10.75, 1.69, -7.27, -18.32, -20.14, -20.38, -20.50, -15.11, -6.95, 0.0}); way_points_y_ = this->declare_parameter>("way_points_y", {0.0, -0.38, -0.57, 2.36, 6.46, 12.54, 13.80, 14.11, 14.49, 15.14, 11.18, 7.24, 1.85, 0.68, 0.34, 0.0}); margin_ = this->declare_parameter("margin", 0.25); + fffff; sleep_time_ = this->declare_parameter("sleep_time", 500000000); sub_map_ = this->create_subscription( From 70867717cd6a1fcef1ecc13d916ae1b602216f30 Mon Sep 17 00:00:00 2001 From: RikuHayakawa Date: Thu, 28 Mar 2024 16:27:40 +0900 Subject: [PATCH 6/8] Revert "wip" This reverts commit 71e65130d274daae1e1e2cd1151ada28729224dc. --- b_global_path_planner/src/b_global_path_planner.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/b_global_path_planner/src/b_global_path_planner.cpp b/b_global_path_planner/src/b_global_path_planner.cpp index e5649f8..7895e72 100644 --- a/b_global_path_planner/src/b_global_path_planner.cpp +++ b/b_global_path_planner/src/b_global_path_planner.cpp @@ -6,7 +6,6 @@ Astar::Astar(): rclcpp::Node("b_global_path_planner") { way_points_x_ = this->declare_parameter>("way_points_x", {0.0, 6.24, 11.92, 12.81, 13.23, 13.60, 10.75, 1.69, -7.27, -18.32, -20.14, -20.38, -20.50, -15.11, -6.95, 0.0}); way_points_y_ = this->declare_parameter>("way_points_y", {0.0, -0.38, -0.57, 2.36, 6.46, 12.54, 13.80, 14.11, 14.49, 15.14, 11.18, 7.24, 1.85, 0.68, 0.34, 0.0}); margin_ = this->declare_parameter("margin", 0.25); - fffff; sleep_time_ = this->declare_parameter("sleep_time", 500000000); sub_map_ = this->create_subscription( From 6e798b6b06763ba1512f5ff9bce143936cc4075c Mon Sep 17 00:00:00 2001 From: RikuHayakawa Date: Fri, 29 Mar 2024 16:42:11 +0900 Subject: [PATCH 7/8] =?UTF-8?q?map=E4=BF=AE=E6=AD=A3=20cherry-pick?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- b_global_path_planner/map/map.pgm | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/b_global_path_planner/map/map.pgm b/b_global_path_planner/map/map.pgm index e65989e..f94d41b 100644 --- a/b_global_path_planner/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 From f3f9e6f07880f1db71795cc6a473c6bb1096a699 Mon Sep 17 00:00:00 2001 From: RikuHayakawa Date: Fri, 5 Apr 2024 05:59:41 +0000 Subject: [PATCH 8/8] =?UTF-8?q?add:=20=E7=B5=8C=E7=94=B1=E7=82=B9=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3(global=5Fpath=E3=81=AE=E7=94=9F=E6=88=90=E3=82=92?= =?UTF-8?q?=E7=A2=BA=E8=AA=8D)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/param/global_path_planner_with_debag.yaml | 4 ++-- b_global_path_planner/src/b_global_path_planner.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) 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 index 8c4a8e5..e308bed 100644 --- 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 @@ -6,8 +6,8 @@ test_show: true sleep_time: 500000000 # 経由点 -way_points_x: [0.0, 6.24, 11.92, 12.81, 13.23, 13.60, 10.75, 1.69, -7.27, -18.32, -20.14, -20.38, -20.50, -15.11, -6.95, 0.0] -way_points_y: [0.0, -0.38, -0.57, 2.36, 6.46, 12.54, 13.80, 14.11, 14.49, 15.14, 11.18, 7.24, 1.85, 0.68, 0.34, 0.0] +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/src/b_global_path_planner.cpp b/b_global_path_planner/src/b_global_path_planner.cpp index 7895e72..a2874ea 100644 --- a/b_global_path_planner/src/b_global_path_planner.cpp +++ b/b_global_path_planner/src/b_global_path_planner.cpp @@ -3,8 +3,8 @@ 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, 6.24, 11.92, 12.81, 13.23, 13.60, 10.75, 1.69, -7.27, -18.32, -20.14, -20.38, -20.50, -15.11, -6.95, 0.0}); - way_points_y_ = this->declare_parameter>("way_points_y", {0.0, -0.38, -0.57, 2.36, 6.46, 12.54, 13.80, 14.11, 14.49, 15.14, 11.18, 7.24, 1.85, 0.68, 0.34, 0.0}); + 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);