diff --git a/b_global_path_planner/CMakeLists.txt b/b_global_path_planner/CMakeLists.txt new file mode 100644 index 0000000..602d839 --- /dev/null +++ b/b_global_path_planner/CMakeLists.txt @@ -0,0 +1,17 @@ +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() \ No newline at end of file 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..ec56418 --- /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 \ No newline at end of file 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..8d1dd37 --- /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 \ No newline at end of file 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..3bba8c1 --- /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 \ No newline at end of file diff --git a/b_global_path_planner/package.xml b/b_global_path_planner/package.xml new file mode 100644 index 0000000..bc2f0ae --- /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 + + + \ No newline at end of file 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..f57c10c --- /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(); + } + +} \ No newline at end of file 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..632783f --- /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 diff --git a/local_goal_creator/CMakeLists.txt b/b_local_goal_creator/CMakeLists.txt similarity index 100% rename from local_goal_creator/CMakeLists.txt rename to b_local_goal_creator/CMakeLists.txt diff --git a/local_goal_creator/config/param/local_goal_creator.yaml b/b_local_goal_creator/config/param/local_goal_creator.yaml similarity index 100% rename from local_goal_creator/config/param/local_goal_creator.yaml rename to b_local_goal_creator/config/param/local_goal_creator.yaml diff --git a/local_goal_creator/config/rviz/local_goal_creator.rviz b/b_local_goal_creator/config/rviz/local_goal_creator.rviz similarity index 100% rename from local_goal_creator/config/rviz/local_goal_creator.rviz rename to b_local_goal_creator/config/rviz/local_goal_creator.rviz diff --git a/local_goal_creator/include/b_local_goal_creator/b_local_goal_creator.hpp b/b_local_goal_creator/include/b_local_goal_creator/b_local_goal_creator.hpp similarity index 91% rename from local_goal_creator/include/b_local_goal_creator/b_local_goal_creator.hpp rename to b_local_goal_creator/include/b_local_goal_creator/b_local_goal_creator.hpp index 2d92490..3756de8 100644 --- a/local_goal_creator/include/b_local_goal_creator/b_local_goal_creator.hpp +++ b/b_local_goal_creator/include/b_local_goal_creator/b_local_goal_creator.hpp @@ -37,8 +37,8 @@ class LocalGoalCreator : public rclcpp::Node rclcpp::Publisher::SharedPtr local_goal_pub_; - nav_msgs::msg::Path path_; geometry_msgs::msg::PointStamped goal_; + std::vector path_; geometry_msgs::msg::PoseStamped pose_; }; diff --git a/local_goal_creator/launch/local_goal_creator.launch b/b_local_goal_creator/launch/local_goal_creator.launch similarity index 100% rename from local_goal_creator/launch/local_goal_creator.launch rename to b_local_goal_creator/launch/local_goal_creator.launch diff --git a/local_goal_creator/launch/local_goal_creator_with_bag.launch b/b_local_goal_creator/launch/local_goal_creator_with_bag.launch similarity index 100% rename from local_goal_creator/launch/local_goal_creator_with_bag.launch rename to b_local_goal_creator/launch/local_goal_creator_with_bag.launch diff --git a/local_goal_creator/package.xml b/b_local_goal_creator/package.xml similarity index 100% rename from local_goal_creator/package.xml rename to b_local_goal_creator/package.xml diff --git a/local_goal_creator/src/b_local_goal_creator.cpp b/b_local_goal_creator/src/b_local_goal_creator.cpp similarity index 78% rename from local_goal_creator/src/b_local_goal_creator.cpp rename to b_local_goal_creator/src/b_local_goal_creator.cpp index 7f258ed..e93b2e0 100644 --- a/local_goal_creator/src/b_local_goal_creator.cpp +++ b/b_local_goal_creator/src/b_local_goal_creator.cpp @@ -33,14 +33,15 @@ LocalGoalCreator::LocalGoalCreator() : Node("LocalGoalCreator") void LocalGoalCreator::poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { pose_ = *msg; + printf("poseCallback\n"); } void LocalGoalCreator::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) { - path_ = *msg; + path_ = msg->poses; is_path_ = true; + printf("pathCallback\n"); } - int LocalGoalCreator::getOdomFreq() { return hz_; } void LocalGoalCreator::process() @@ -57,23 +58,27 @@ void LocalGoalCreator::process() void LocalGoalCreator::publishGoal() { double distance = getDistance(); - if(distance < taeget_distance_) + while(distance < taeget_distance_) { goal_index_ += index_step_; - if(goal_index_ >= path_.poses.size()) + distance = getDistance(); + if(goal_index_ >= path_.size()) { - goal_index_ = path_.poses.size() - 1; + goal_index_ = path_.size() - 1; + break; } } + goal_.point.x = path_[goal_index_].pose.position.x; + goal_.point.y = path_[goal_index_].pose.position.y; goal_.header.stamp = this->get_clock()->now(); - goal_.point = path_.poses[goal_index_].pose.position; + printf("pub\n"); local_goal_pub_->publish(goal_); } double LocalGoalCreator::getDistance() { - double dx = path_.poses[goal_index_].pose.position.x - pose_.pose.position.x; - double dy = path_.poses[goal_index_].pose.position.y - pose_.pose.position.y; + double dx = path_[goal_index_].pose.position.x - pose_.pose.position.x; + double dy = path_[goal_index_].pose.position.y - pose_.pose.position.y; return hypot(dx, dy); } diff --git a/local_goal_creator/src/b_local_goal_creator_node.cpp b/b_local_goal_creator/src/b_local_goal_creator_node.cpp similarity index 100% rename from local_goal_creator/src/b_local_goal_creator_node.cpp rename to b_local_goal_creator/src/b_local_goal_creator_node.cpp diff --git a/b_local_map_creator/CMakeLists.txt b/b_local_map_creator/CMakeLists.txt index 40844f7..c3b2d6f 100644 --- a/b_local_map_creator/CMakeLists.txt +++ b/b_local_map_creator/CMakeLists.txt @@ -7,8 +7,8 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() # 実行ファイルを作成する ament_auto_add_executable(${PROJECT_NAME}_node - src/chibi24_b/b_local_map_creator.cpp - src/chibi24_b/b_local_map_creator_node.cpp) + src/b_local_map_creator.cpp + src/b_local_map_creator_node.cpp) # launchファイルをインストール（ament_package_indexが探せる場所に置く） install(DIRECTORY launch diff --git a/b_local_map_creator/src/b_local_map_creator.cpp b/b_local_map_creator/src/b_local_map_creator.cpp index 57eb1ec..543f29b 100644 --- a/b_local_map_creator/src/b_local_map_creator.cpp +++ b/b_local_map_creator/src/b_local_map_creator.cpp @@ -10,7 +10,7 @@ LocalMapCreator::LocalMapCreator() : Node("b_local_map_creator") local_map_resolution_ = this->declare_parameter("map_resolution",0.025); //printf("map_resolution = %f\n",local_map_resolution_); - sub_obs_poses_ = this->create_subscription("/obstacle/pose",rclcpp::QoS(1).reliable(),std::bind(&LocalMapCreator::obs_poses_callback,this,std::placeholders::_1)); + sub_obs_poses_ = this->create_subscription("/obstacle_pose",rclcpp::QoS(1).reliable(),std::bind(&LocalMapCreator::obs_poses_callback,this,std::placeholders::_1)); pub_local_map_ = this->create_publisher("/local/map",rclcpp::QoS(1).reliable()); local_map_.info.resolution = local_map_resolution_; diff --git a/b_local_path_planner/include/b_local_path_planner/b_local_path_planner.hpp b/b_local_path_planner/include/b_local_path_planner/b_local_path_planner.hpp index 358f612..3743d33 100644 --- a/b_local_path_planner/include/b_local_path_planner/b_local_path_planner.hpp +++ b/b_local_path_planner/include/b_local_path_planner/b_local_path_planner.hpp @@ -107,10 +107,10 @@ class DWAPlanner : public rclcpp::Node //tf2_ros::Buffer tfBuffer_; //tf2_ros::TransformListener tfListener_; - std::unique_ptr tfBuffer_; - std::shared_ptr tfListener_; - //std::shared_ptr tfBuffer_; + //std::unique_ptr tfBuffer_; //std::shared_ptr tfListener_; + std::shared_ptr tfBuffer_; + std::shared_ptr tfListener_; //tfBuffer_.setUsingDedicatedThread(true); diff --git a/b_local_path_planner/launch/b_local_path_planner.launch b/b_local_path_planner/launch/b_local_path_planner.launch deleted file mode 100644 index e69de29..0000000 diff --git a/b_local_path_planner/launch/b_local_path_planner.launch.py b/b_local_path_planner/launch/b_local_path_planner.launch.py new file mode 100644 index 0000000..f14938e --- /dev/null +++ b/b_local_path_planner/launch/b_local_path_planner.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='b_obstacle_detector', + executable='b_obstacle_detector_node', + #output='screen', + parameters=[{'use_sim_time': True}] + ), + Node( + package='b_local_map_creator', + executable='b_local_map_creator_node', + #output='screen', + parameters=[{'use_sim_time': True}] + ), + Node( + package='b_localizer', + executable='b_localizer_node', + #output='screen', + parameters=[{'use_sim_time': True}] + ), + Node( + package='b_global_path_planner', + executable='b_global_path_planner_node', + #output='screen', + parameters=[{'use_sim_time': True}] + ), + Node( + package='b_local_goal_creator', + executable='b_local_goal_creator_node', + #output='screen', + parameters=[{'use_sim_time': True}] + ), + Node( + package='b_local_path_planner', + executable='b_local_path_planner_node', + output='screen', + parameters=[{'use_sim_time': True}] + ), + + ]) \ No newline at end of file diff --git a/b_local_path_planner/src/b_local_path_planner.cpp b/b_local_path_planner/src/b_local_path_planner.cpp index 81e802f..3a0b880 100644 --- a/b_local_path_planner/src/b_local_path_planner.cpp +++ b/b_local_path_planner/src/b_local_path_planner.cpp @@ -6,21 +6,21 @@ DWAPlanner::DWAPlanner() : Node("b_local_path_planner") this->declare_parameter("hz", 50); this->declare_parameter("dt", 0.1); //変更の余地アリ this->declare_parameter("goal_tolerance", 0.3); //変更の余地あり - this->declare_parameter("max_vel", 0.45); + this->declare_parameter("max_vel", 0.4); this->declare_parameter("min_vel", 0.0); - this->declare_parameter("max_yawrate", 1.0); - this->declare_parameter("min_yawrate", -1.0); + this->declare_parameter("max_yawrate", 0.5); + this->declare_parameter("min_yawrate", -0.5); this->declare_parameter("max_accel", 1000.0); this->declare_parameter("max_dyawrate", 1000.0); this->declare_parameter("v_reso", 0.05); - this->declare_parameter("y_reso", 0.1); - this->declare_parameter("predict_time", 2.0); //変更の余地あり - this->declare_parameter("heading_cost_gain", 1.0); - this->declare_parameter("velocity_cost_gain", 1.0); - this->declare_parameter("distance_cost_gain", 1.0); - this->declare_parameter("robot_radius", 0.25); //いらない - this->declare_parameter("radius_margin", 0.1); //いらない - this->declare_parameter("search_range", 0.95); //変更の余地あり + this->declare_parameter("y_reso", 0.02); + this->declare_parameter("predict_time",3.0); // 変更の余地あり + this->declare_parameter("heading_cost_gain", 0.7); + this->declare_parameter("velocity_cost_gain", 0.6); + this->declare_parameter("distance_cost_gain", 0.8); + this->declare_parameter("robot_radius", 0.25); // いらない + this->declare_parameter("radius_margin", 0.1); // いらない + this->declare_parameter("search_range", 0.95); // 変更の余地あり // パラメータの取得 this->get_parameter("hz", hz_); @@ -41,31 +41,29 @@ DWAPlanner::DWAPlanner() : Node("b_local_path_planner") this->get_parameter("robot_radius", robot_radius_); this->get_parameter("radius_margin",radius_margin_); this->get_parameter("search_range", search_range_); - //this->declare_parameter("robot_frame","base_link"); - printf("hz =%d\n",hz_); - printf("dt =%f\n",dt_); - printf("predict_time =%f\n",predict_time_); - printf("goal_tolerance =%f\n",goal_tolerance_); - printf("max_vel =%f\n",max_vel_); - printf("min_vel =%f\n",min_vel_); - printf("max_yawrate =%f\n",max_yawrate_); - printf("min_yawrate =%f\n",min_yawrate_); - printf("max_accel =%f\n",max_accel_); - printf("max_dyawrate =%f\n",max_dyawrate_); - printf("v_reso =%f\n",v_reso_); - printf("y_reso =%f\n",y_reso_); - printf("heading_cost_gain =%f\n",heading_cost_gain_); - printf("velocity_cost_gain =%f\n",velocity_cost_gain_); - printf("distance_cost_gain =%f\n",distance_cost_gain_); - printf("serch_range =%f\n",search_range_); + // this->declare_parameter("robot_frame","base_link"); + + // printf("hz =%d\n",hz_); + // printf("dt =%f\n",dt_); + // printf("predict_time =%f\n",predict_time_); + // printf("goal_tolerance =%f\n",goal_tolerance_); + //printf("max_vel =%f\n",max_vel_); + // printf("min_vel =%f\n",min_vel_); + // printf("max_yawrate =%f\n",max_yawrate_); + // printf("min_yawrate =%f\n",min_yawrate_); + // printf("max_accel =%f\n",max_accel_); + // printf("max_dyawrate =%f\n",max_dyawrate_); + // printf("v_reso =%f\n",v_reso_); + // printf("y_reso =%f\n",y_reso_); + // printf("heading_cost_gain =%f\n",heading_cost_gain_); + // printf("velocity_cost_gain =%f\n",velocity_cost_gain_); + // printf("distance_cost_gain =%f\n",distance_cost_gain_); + // printf("serch_range =%f\n",search_range_); - - - // Subscriber local_goal_sub_ = this->create_subscription("/local_goal", rclcpp::QoS(1).reliable(), std::bind(&DWAPlanner::local_goal_callback, this, std::placeholders::_1)); - obs_pose_sub_ = this->create_subscription("/obstacle/pose", rclcpp::QoS(1).reliable(), std::bind(&DWAPlanner::obs_pose_callback, this, std::placeholders::_1)); + obs_pose_sub_ = this->create_subscription("/obstacle_pose", rclcpp::QoS(1).reliable(), std::bind(&DWAPlanner::obs_pose_callback, this, std::placeholders::_1)); // Publisher cmd_speed_pub_ = this->create_publisher("/roomba/control", rclcpp::QoS(1).reliable()); @@ -73,7 +71,7 @@ DWAPlanner::DWAPlanner() : Node("b_local_path_planner") optimal_path_pub_ = this->create_publisher("/optimal/path", rclcpp::QoS(1).reliable()); - tfBuffer_ = std::make_unique(this->get_clock()); + tfBuffer_ = std::make_shared(this->get_clock()); tfListener_ = std::make_shared(*tfBuffer_); //predict_path_.header.frame_id ="base_link"; //optimal_path_.header.frame_id ="base_link"; @@ -88,7 +86,9 @@ void DWAPlanner::local_goal_callback(const geometry_msgs::msg::PointStamped::Sha { //lookupTransform("変換のベースとなる座標系","変更したい対象の座標系",変更したい時間(過去データを扱う場合は注意が必要)) //transformStamped = tfBuffer_->lookupTransform(this->get_parameter("robot_frame").as_string(),"map", tf2::TimePointZero); //座標系の変換 + printf("1\n"); transformStamped = tfBuffer_->lookupTransform("base_link","map", tf2::TimePointZero); //座標系の変換 + printf("2\n"); // 取得した変換情報を表示 RCLCPP_INFO(this->get_logger(), "Transform: [%f, %f, %f]", transformStamped.transform.translation.x, transformStamped.transform.translation.y, transformStamped.transform.translation.z); flag_local_goal_ = true; @@ -297,6 +297,10 @@ double DWAPlanner::calc_eval(const std::vector &trajectory) //評価関 // ROS_INFO_STREAM("velocity: " << velocity); // ROS_INFO_STREAM("distance: " << distance); + // RCLCPP_INFO(rclcpp::get_logger("b_local_path_planner"),"heading: %f" , heading_cost_gain_*heading); + // RCLCPP_INFO(rclcpp::get_logger("b_local_path_planner"),"velocity: %f" , velocity_cost_gain_*velocity); + // RCLCPP_INFO(rclcpp::get_logger("b_local_path_planner"),"distance: %f" , distance_cost_gain_*distance); + return heading_cost_gain_ * heading + velocity_cost_gain_ * velocity + distance_cost_gain_ * distance; } diff --git a/b_local_path_planner/src/b_local_path_planner_node.cpp b/b_local_path_planner/src/b_local_path_planner_node.cpp index 8c2c60e..5da7602 100644 --- a/b_local_path_planner/src/b_local_path_planner_node.cpp +++ b/b_local_path_planner/src/b_local_path_planner_node.cpp @@ -10,8 +10,8 @@ int main(int argc, char *argv[]) while (rclcpp::ok()) { node->process(); rclcpp::spin_some(node); - loop_rate.sleep(); - //RCLCPP_ERROR(node->get_logger(), "Error Message"); + loop_rate.sleep(); + //RCLCPP_ERROR(node->get_logger(), "Error Message"); } rclcpp::shutdown(); return 0; diff --git a/b_localizer/package.xml b/b_localizer/package.xml index 2b9ebf9..7413986 100644 --- a/b_localizer/package.xml +++ b/b_localizer/package.xml @@ -2,7 +2,7 @@ b_localizer 0.0.0 - The localizer package + the b_localizer package diff --git a/b_localizer/src/b_localizer.cpp b/b_localizer/src/b_localizer.cpp index ed48a88..e16b21a 100644 --- a/b_localizer/src/b_localizer.cpp +++ b/b_localizer/src/b_localizer.cpp @@ -9,7 +9,7 @@ EMCL::EMCL():Node("b_localizer") { // パラメータの設定(EMCL) this -> declare_parameter("flag_init_noise", true); - this -> declare_parameter("flag_broadcast", true); + this -> declare_parameter("flag_broadcast", true); //議論の余地あり this -> declare_parameter("is_visible", true); this -> declare_parameter("hz", 10); this -> declare_parameter("particle_num", 500); @@ -102,13 +102,13 @@ void EMCL::map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { map_ = *msg; flag_map_ = true; - printf("m_c\n"); + //printf("m_c\n"); } // odometryのコールバック関数 void EMCL::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { - printf("o_c\n"); + //printf("o_c\n"); prev_odom_ = last_odom_; last_odom_ = *msg; flag_odom_ = true; @@ -126,7 +126,7 @@ void EMCL::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) // laserのコールバック関数 void EMCL::laser_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { - printf("l_c\n"); + //printf("l_c\n"); laser_ = *msg; flag_laser_ = true; } @@ -136,25 +136,25 @@ int EMCL::getOdomFreq() { return hz_; } // 唯一，main文で実行する関数 void EMCL::process() { // パーティクルの初期化 - printf("plocess_0\n"); + //printf("plocess_0\n"); if(flag_map_ and flag_odom_ and flag_laser_) { - printf("plocess_1\n"); + //printf("plocess_1\n"); broadcast_odom_state(); // map座標系とodom座標系の関係を報告 if(flag_move_) { localize(); // 自己位置推定 - printf("localize\n"); + //printf("localize\n"); } else { publish_estimated_pose(); // 推定位置のパブリッシュ publish_particles(); // パーティクルクラウドのパブリッシュ - printf("publish_\n"); + //printf("publish_\n"); } } - printf("process_3\n"); + //printf("process_3\n"); } // パーティクル，推定位置の初期化 @@ -164,7 +164,7 @@ void EMCL::initialize() estimated_pose_.set(init_x_, init_y_, init_yaw_); Particle particle; - printf("!0\n"); + //printf("!0\n"); for(int i=0; i norm_dist(mean, stddev); - printf("norm_rv"); + //printf("norm_rv"); return norm_dist(engine_); } @@ -206,9 +206,9 @@ void EMCL::reset_weight() { for(auto& p : particles_){ p.set_weight(1.0/particles_.size()); - printf("\n"); + //printf("\n"); } - printf("reset_weight\n"); + //printf("reset_weight\n"); } // map座標系とodom座標系の関係を報告 @@ -245,7 +245,8 @@ void EMCL::broadcast_odom_state() geometry_msgs::msg::TransformStamped odom_state; // 現在の時間の格納 - odom_state.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); + //odom_state.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); + odom_state.header.stamp = get_clock()->now(); // 親フレーム・子フレームの指定 odom_state.header.frame_id = map_.header.frame_id; @@ -262,7 +263,7 @@ void EMCL::broadcast_odom_state() // tf情報をbroadcast(座標系の設定) odom_state_broadcaster.sendTransform(odom_state); // odom_state_broadcaster_.sendTransform(odom_state); - printf("broadcast_odom_state\n"); + //printf("broadcast_odom_state\n"); } } @@ -271,7 +272,7 @@ double EMCL::normalize_angle(double angle) { while(M_PI < angle) angle -= 2.0*M_PI; while(angle < -M_PI) angle += 2.0*M_PI; - printf("norm_angle\n"); + //printf("norm_angle\n"); return angle; } @@ -283,7 +284,7 @@ void EMCL::localize() observation_update(); // 観測更新（位置推定・リサンプリングを含む） publish_estimated_pose(); // 推定位置のパブリッシュ publish_particles(); // パーティクルクラウドのパブリッシュ - printf("localize\n"); + //printf("localize\n"); } // 動作更新 @@ -309,7 +310,7 @@ void EMCL::motion_update() for(auto& p : particles_){ p.pose_.move(length, direction, dyaw, odom_model_.get_fw_noise(), odom_model_.get_rot_noise()); } - printf("motion_update\n"); + //printf("motion_update\n"); } // 観測更新（位置推定・リサンプリングを含む） @@ -339,7 +340,7 @@ void EMCL::observation_update() resampling(); // リサンプリング reset_counter = 0; } - printf("observation_update"); + //printf("observation_update"); } // 周辺尤度の算出 @@ -349,7 +350,7 @@ double EMCL::calc_marginal_likelihood() for(const auto& p : particles_){ sum += p.weight(); } - printf("calc_marginal_likelihood\n"); + //printf("calc_marginal_likelihood\n"); return sum; } @@ -360,7 +361,7 @@ void EMCL::estimate_pose() weighted_mean_pose(); // 加重平均 // max_weight_pose(); // 最大の重みを有するポーズ // median_pose(); // 中央値 - printf("estimate_pose\n"); + //printf("estimate_pose\n"); } // 推定位置の決定（平均） @@ -407,7 +408,7 @@ void EMCL::weighted_mean_pose() } estimated_pose_.set(x_mean, y_mean, yaw_mean); - printf("weighted_mean_pose\n"); + //printf("weighted_mean_pose\n"); } // 重みの正規化 @@ -420,7 +421,7 @@ void EMCL::normalize_belief() for(auto& p : particles_){ p.set_weight(p.weight() / weight_sum); } - printf("noomalize_belief\n"); + //printf("noomalize_belief\n"); } // 推定位置の決定（最大の重みを有するポーズ） @@ -456,7 +457,7 @@ void EMCL::median_pose() const double y_median = get_median(y_list); const double yaw_median = get_median(yaw_list); estimated_pose_.set(x_median, y_median, yaw_median); - printf("median_pose\n"); + //printf("median_pose\n"); } // 配列の中央値を返す @@ -468,7 +469,7 @@ double EMCL::get_median(std::vector& data) }else{ return (data[data.size()/2 - 1] + data[data.size()/2]) / 2.0; } - printf("get_mediam\n"); + //printf("get_mediam\n"); } // 膨張リセット @@ -486,7 +487,7 @@ void EMCL::expansion_resetting() // 重みを初期化 reset_weight(); - printf("expansion_resetting\n"); + //printf("expansion_resetting\n"); } // リサンプリング（系統サンプリング） @@ -527,7 +528,7 @@ void EMCL::resampling() // 重みを初期化 reset_weight(); - printf("resampling\n"); + //printf("resampling\n"); } // 推定位置のパブリッシュ @@ -542,7 +543,7 @@ void EMCL::publish_estimated_pose() tf2::convert(q,estimated_pose_msg_.pose.orientation); pub_estimated_pose_ -> publish(estimated_pose_msg_); - printf("publish_estimated_pose\n"); + //printf("publish_estimated_pose\n"); } // パーティクルクラウドのパブリッシュ @@ -569,5 +570,5 @@ void EMCL::publish_particles() pub_particle_cloud_ -> publish(particle_cloud_msg_); } - printf("publish_particles\n"); + //printf("publish_particles\n"); } \ No newline at end of file diff --git a/b_obstacle_detector/src/b_obstacle_detector.cpp b/b_obstacle_detector/src/b_obstacle_detector.cpp index e970571..20d6cad 100644 --- a/b_obstacle_detector/src/b_obstacle_detector.cpp +++ b/b_obstacle_detector/src/b_obstacle_detector.cpp @@ -8,11 +8,11 @@ ObstacleDetector::ObstacleDetector() //auto hz_ = this->get_parameter("hz").as_int(); ignore_dist_ = this->declare_parameter("ignore_dist",0.01); laser_step_ = this->declare_parameter("laser_step",3); - ignore_angle_range_list_ = this->declare_parameter>("ignore_angle_range_list",{(3.0*M_PI/16.0), (5.0*M_PI/16.0), (11.0*M_PI/16.0)}); + ignore_angle_range_list_ = this->declare_parameter>("ignore_angle_range_list",{(2.0*M_PI/16.0), (6.0*M_PI/16.0), (10.0*M_PI/16.0)}); //printf("ignore_angle_range_list = %f\n",ignore_angle_range_list_[0]); scan_sub_ = this->create_subscription("/scan",rclcpp::QoS(1).reliable(),std::bind(&ObstacleDetector::scan_callback,this,std::placeholders::_1)); - obstacle_pose_pub_ = this->create_publisher("/obstacle/pose",rclcpp::QoS(1).reliable()); + obstacle_pose_pub_ = this->create_publisher("/obstacle_pose",rclcpp::QoS(1).reliable()); // scan_ を初期化する scan_ = std::nullopt; diff --git a/local_goal_creator/include/local_goal_creator/local_goal_creator.h b/local_goal_creator/include/local_goal_creator/local_goal_creator.h deleted file mode 100644 index bb7ce22..0000000 --- a/local_goal_creator/include/local_goal_creator/local_goal_creator.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef LOCAL_GOAL_CREATOR_H -#define LOCAL_GOAL_CREATOR_H - -#include -#include -#include -#include - -class LocalGoalCreator -{ -public: - LocalGoalCreator(); - void process(); - -private: - void pathCallback(const nav_msgs::Path::ConstPtr& msg); - void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg); - void publishGoal(); - double getDistance(); - - int hz_; - int index_step_; - double goal_index_; - double taeget_distance_; - bool is_path_ = false; - - ros::NodeHandle nh_; - ros::NodeHandle private_nh_; - ros::Subscriber path_sub_; - ros::Subscriber pose_sub_; - ros::Publisher local_goal_pub_; - - nav_msgs::Path path_; - geometry_msgs::PointStamped goal_; - geometry_msgs::PoseStamped pose_; -}; - -#endif // LOCAL_GOAL_CREATOR_H \ No newline at end of file diff --git a/local_goal_creator/include/local_goal_creator/local_goal_creator.hpp b/local_goal_creator/include/local_goal_creator/local_goal_creator.hpp deleted file mode 100644 index f18b1b7..0000000 --- a/local_goal_creator/include/local_goal_creator/local_goal_creator.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef LOCAL_GOAL_CREATOR_H -#define LOCAL_GOAL_CREATOR_H - -#include -#include -#include -#include - -class LocalGoalCreator : public rclcpp::Node -{ -public: - LocalGoalCreator(); - void process(); - int getOdomFreq(); - -private: - void pathCallback(const nav_msgs::msg::Path::SharedPtr msg); - void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - void publishGoal(); - double getDistance(); - - int hz_; - int index_step_; - int goal_index_; - double taeget_distance_; - bool is_path_ = false; - - //　 - rclcpp::Node::SharedPtr nh_; - rclcpp::Node::SharedPtr private_nh_; - - //　Subscriber - rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr pose_sub_; - - // Publisher - rclcpp::Publisher::SharedPtr local_goal_pub_; - - - nav_msgs::msg::Path path_; - geometry_msgs::msg::PointStamped goal_; - geometry_msgs::msg::PoseStamped pose_; -}; - -#endif // LOCAL_GOAL_CREATOR_H \ No newline at end of file diff --git a/local_goal_creator/src/local_goal_creator.cpp b/local_goal_creator/src/local_goal_creator.cpp deleted file mode 100644 index 8a1ec2d..0000000 --- a/local_goal_creator/src/local_goal_creator.cpp +++ /dev/null @@ -1,79 +0,0 @@ -#include "local_goal_creator/local_goal_creator.hpp" - -LocalGoalCreator::LocalGoalCreator() : Node("LocalGoalCreator") -{ - this -> declare_parameter("hz", 10); - this -> declare_parameter("index_step", 5); - this -> declare_parameter("target_distance", 50); - this -> declare_parameter("goal_index", 1.7); - - this -> get_parameter("hz", hz_); - this -> get_parameter("index_step", index_step_); - this -> get_parameter("target_distance", taeget_distance_); - this -> get_parameter("goal_index", goal_index_); - - goal_.header.frame_id = "map"; - - path_sub_ = this->create_subscription( - "/global_path", rclcpp::QoS(1).reliable(), - std::bind(&LocalGoalCreator::pathCallback, this, std::placeholders::_1)); - - pose_sub_ = this->create_subscription( - "/estimate_pose", rclcpp::QoS(1).reliable(), - std::bind(&LocalGoalCreator::poseCallback, this, std::placeholders::_1)); - - local_goal_pub_ = this->create_publisher( - "/local_goal", rclcpp::QoS(1).reliable()); - - //path_sub_ = nh_.subscribe("/global_path", 1, &LocalGoalCreator::pathCallback, this); - //pose_sub_ = nh_.subscribe("/estimate_pose", 1, &LocalGoalCreator::poseCallback, this); - //local_goal_pub_ = nh_.advertise("/local_goal", 1); -} - -void LocalGoalCreator::poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) -{ - pose_ = *msg; -} - -void LocalGoalCreator::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) -{ - path_ = *msg; - is_path_ = true; -} - -int LocalGoalCreator::getOdomFreq() { return hz_; } - -void LocalGoalCreator::process() -{ - - - if(is_path_) - { - publishGoal(); - } - -} - -void LocalGoalCreator::publishGoal() -{ - double distance = getDistance(); - if(distance < taeget_distance_) - { - goal_index_ += index_step_; - if(goal_index_ >= path_.poses.size()) - { - goal_index_ = path_.poses.size() - 1; - } - } - goal_.header.stamp = this->get_clock()->now(); - goal_.point = path_.poses[goal_index_].pose.position; - local_goal_pub_->publish(goal_); -} - -double LocalGoalCreator::getDistance() -{ - double dx = path_.poses[goal_index_].pose.position.x - pose_.pose.position.x; - double dy = path_.poses[goal_index_].pose.position.y - pose_.pose.position.y; - return hypot(dx, dy); -} - diff --git a/local_goal_creator/src/local_goal_creator_node.cpp b/local_goal_creator/src/local_goal_creator_node.cpp deleted file mode 100644 index 7427539..0000000 --- a/local_goal_creator/src/local_goal_creator_node.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "local_goal_creator/local_goal_creator.hpp" - -int main(int argc, char* argv[]) -{ - - rclcpp::init(argc, argv); // ノードの初期化 - auto node = std::make_shared(); - rclcpp::Rate loop_rate(node->getOdomFreq()); - - - while(rclcpp::ok()){ - node->process(); - rclcpp::spin_some(node); // コールバック関数の実行 - loop_rate.sleep(); // 周期が終わるまで待つ - } - - return 0; -} \ No newline at end of file