From 132ca2dd3cb8daca98c979108ad0be733e462d94 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 3 Dec 2023 16:17:08 +0900 Subject: [PATCH 01/15] =?UTF-8?q?=E3=82=B0=E3=83=AA=E3=83=83=E3=83=89?= =?UTF-8?q?=E3=83=9E=E3=83=83=E3=83=97=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_local_planner/local_planner.hpp | 15 ++++++++++++++- crane_local_planner/package.xml | 2 ++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/crane_local_planner/include/crane_local_planner/local_planner.hpp b/crane_local_planner/include/crane_local_planner/local_planner.hpp index a0710ce97..f95e8b99b 100644 --- a/crane_local_planner/include/crane_local_planner/local_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/local_planner.hpp @@ -17,6 +17,7 @@ #include "RVO.h" #include "visibility_control.h" +#include namespace crane { @@ -32,7 +33,7 @@ class LocalPlannerComponent : public rclcpp::Node public: COMPOSITION_PUBLIC explicit LocalPlannerComponent(const rclcpp::NodeOptions & options) - : rclcpp::Node("local_planner", options) + : rclcpp::Node("local_planner", options), map({"penalty", "ball_placement", "theirs", "ours", "ball"}) { declare_parameter("enable_rvo", true); enable_rvo = get_parameter("enable_rvo").as_bool(); @@ -79,6 +80,13 @@ class LocalPlannerComponent : public rclcpp::Node RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST, RVO_RADIUS, RVO_MAX_SPEED); + + // MAP_RESOLUTION = get_parameter("map_resolution").as_double(); + declare_parameter("map_resolution", MAP_RESOLUTION); + MAP_RESOLUTION = get_parameter("map_resolution").as_double(); + map.setFrameId("map"); + map.setGeometry(grid_map::Length(1.2, 2.0), MAP_RESOLUTION, grid_map::Position(0.0, 0.0)); + world_model = std::make_shared(*this); // TODO(HansRobo): add goal area as obstacles @@ -134,6 +142,11 @@ class LocalPlannerComponent : public rclcpp::Node double NON_RVO_P_GAIN = 4.0; double NON_RVO_I_GAIN = 0.0; double NON_RVO_D_GAIN = 0.0; + + grid_map::GridMap map; + + double MAP_RESOLUTION = 0.05; + }; } // namespace crane diff --git a/crane_local_planner/package.xml b/crane_local_planner/package.xml index a7e5bfaea..7287eca9a 100755 --- a/crane_local_planner/package.xml +++ b/crane_local_planner/package.xml @@ -16,6 +16,8 @@ rclcpp_components rvo2_vendor std_msgs + grid_map_core + grid_map_ros ament_lint_auto crane_lint_common From 0830813f4f5ca63ec25c48bef37ab95a096183a8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 3 Dec 2023 20:50:30 +0900 Subject: [PATCH 02/15] =?UTF-8?q?inline=E5=BF=98=E3=82=8C=E3=81=A6?= =?UTF-8?q?=E3=81=9F=E3=80=80=E3=81=A6=E3=81=B8=E3=81=BA=E3=82=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_geometry/geometry_operations.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp b/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp index 19400c3fe..836fc18ea 100644 --- a/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp +++ b/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp @@ -49,7 +49,7 @@ inline double getAngleDiff(double angle_rad1, double angle_rad2) } } -double getIntermediateAngle(double angle_rad1, double angle_rad2) +inline double getIntermediateAngle(double angle_rad1, double angle_rad2) { angle_rad1 = normalizeAngle(angle_rad1); angle_rad2 = normalizeAngle(angle_rad2); From 65dda85e3507f19ac36398142483c8ab17e7adda Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 3 Dec 2023 20:52:32 +0900 Subject: [PATCH 03/15] =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E?= =?UTF-8?q?=E3=83=83=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_local_planner/local_planner.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/crane_local_planner/include/crane_local_planner/local_planner.hpp b/crane_local_planner/include/crane_local_planner/local_planner.hpp index f95e8b99b..018eba143 100644 --- a/crane_local_planner/include/crane_local_planner/local_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/local_planner.hpp @@ -12,12 +12,12 @@ #include #include #include +#include #include #include #include "RVO.h" #include "visibility_control.h" -#include namespace crane { @@ -33,7 +33,8 @@ class LocalPlannerComponent : public rclcpp::Node public: COMPOSITION_PUBLIC explicit LocalPlannerComponent(const rclcpp::NodeOptions & options) - : rclcpp::Node("local_planner", options), map({"penalty", "ball_placement", "theirs", "ours", "ball"}) + : rclcpp::Node("local_planner", options), + map({"penalty", "ball_placement", "theirs", "ours", "ball"}) { declare_parameter("enable_rvo", true); enable_rvo = get_parameter("enable_rvo").as_bool(); @@ -80,7 +81,6 @@ class LocalPlannerComponent : public rclcpp::Node RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST, RVO_RADIUS, RVO_MAX_SPEED); - // MAP_RESOLUTION = get_parameter("map_resolution").as_double(); declare_parameter("map_resolution", MAP_RESOLUTION); MAP_RESOLUTION = get_parameter("map_resolution").as_double(); @@ -146,7 +146,6 @@ class LocalPlannerComponent : public rclcpp::Node grid_map::GridMap map; double MAP_RESOLUTION = 0.05; - }; } // namespace crane From 39a8e15cb9adb1a8e901a729ff5af0eee2298eff Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 3 Dec 2023 21:06:50 +0900 Subject: [PATCH 04/15] =?UTF-8?q?float=E3=81=98=E3=82=83=E3=81=AA=E3=81=8F?= =?UTF-8?q?=E3=81=A6double=E3=82=92=E4=BD=BF=E3=81=86=E4=BC=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_geometry/boost_geometry.hpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp b/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp index f4c0a6fe1..8421a1001 100755 --- a/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp +++ b/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp @@ -18,10 +18,10 @@ #include namespace bg = boost::geometry; -using Vector2 = Eigen::Vector2f; -using Point = Eigen::Vector2f; -using Velocity = Eigen::Vector2f; -using Accel = Eigen::Vector2f; +using Vector2 = Eigen::Vector2d; +using Point = Eigen::Vector2d; +using Velocity = Eigen::Vector2d; +using Accel = Eigen::Vector2d; using Segment = bg::model::segment; using Polygon = bg::model::polygon; using LineString = bg::model::linestring; @@ -31,17 +31,17 @@ using ClosestPoint = bg::closest_point_result; struct Circle { Point center; - float radius; + double radius; }; namespace boost::geometry { template -float distance(const Circle & circle, const Geometry1 & geometry1) +double distance(const Circle & circle, const Geometry1 & geometry1) { - float dist = distance(circle.center, geometry1) - circle.radius; + double dist = distance(circle.center, geometry1) - circle.radius; if (dist < 0) { - return 0.0f; + return 0.; } return dist; } @@ -50,13 +50,13 @@ float distance(const Circle & circle, const Geometry1 & geometry1) struct Pose2D { Point pos; - float theta; + double theta; }; struct Velocity2D { Point linear; - float omega; + double omega; }; struct Rect From 6983d9ca0b5275a9100c1cb44b59735f9033a58f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 3 Dec 2023 23:01:57 +0900 Subject: [PATCH 05/15] =?UTF-8?q?=E7=AC=AC2=E5=9B=9Efloat=E3=81=98?= =?UTF-8?q?=E3=82=83=E3=81=AA=E3=81=8F=E3=81=A6double=E3=82=92=E4=BD=BF?= =?UTF-8?q?=E3=81=86=E4=BC=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_geometry/boost_geometry.hpp | 2 ++ .../include/crane_geometry/eigen_adapter.hpp | 26 ++++++++-------- .../include/crane_geometry/interval.hpp | 30 +++++++++---------- .../field_analysis_wrapper.hpp | 2 +- .../play_situation_wrapper.hpp | 2 +- 5 files changed, 33 insertions(+), 29 deletions(-) diff --git a/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp b/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp index 8421a1001..4313e10fc 100755 --- a/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp +++ b/utility/crane_geometry/include/crane_geometry/boost_geometry.hpp @@ -17,6 +17,8 @@ #include #include +#include "eigen_adapter.hpp" + namespace bg = boost::geometry; using Vector2 = Eigen::Vector2d; using Point = Eigen::Vector2d; diff --git a/utility/crane_geometry/include/crane_geometry/eigen_adapter.hpp b/utility/crane_geometry/include/crane_geometry/eigen_adapter.hpp index 08d4b45fd..652734c36 100644 --- a/utility/crane_geometry/include/crane_geometry/eigen_adapter.hpp +++ b/utility/crane_geometry/include/crane_geometry/eigen_adapter.hpp @@ -7,46 +7,48 @@ #ifndef CRANE_GEOMETRY__EIGEN_ADAPTER_HPP_ #define CRANE_GEOMETRY__EIGEN_ADAPTER_HPP_ -#include "boost_geometry.hpp" +#include +#include namespace boost::geometry::traits { template <> -struct tag +struct tag { typedef point_tag type; }; + template <> -struct coordinate_type +struct coordinate_type { - typedef float type; + typedef double type; }; template <> -struct coordinate_system +struct coordinate_system { typedef cs::cartesian type; }; template <> -struct dimension : boost::mpl::int_<2> +struct dimension : boost::mpl::int_<2> { }; template <> -struct access +struct access { - static float get(Eigen::Vector2f const & p) { return p.x(); } + static double get(Eigen::Vector2d const & p) { return p.x(); } - static void set(Eigen::Vector2f & p, float const & value) { p.x() = value; } + static void set(Eigen::Vector2d & p, double const & value) { p.x() = value; } }; template <> -struct access +struct access { - static float get(Eigen::Vector2f const & p) { return p.y(); } + static double get(Eigen::Vector2d const & p) { return p.y(); } - static void set(Eigen::Vector2f & p, float const & value) { p.y() = value; } + static void set(Eigen::Vector2d & p, double const & value) { p.y() = value; } }; } // namespace boost::geometry::traits #endif // CRANE_GEOMETRY__EIGEN_ADAPTER_HPP_ diff --git a/utility/crane_geometry/include/crane_geometry/interval.hpp b/utility/crane_geometry/include/crane_geometry/interval.hpp index c473ae43d..53b10c903 100644 --- a/utility/crane_geometry/include/crane_geometry/interval.hpp +++ b/utility/crane_geometry/include/crane_geometry/interval.hpp @@ -13,18 +13,18 @@ class Interval { private: - std::vector uppers; - std::vector lowers; + std::vector uppers; + std::vector lowers; public: Interval() {} ~Interval() {} - void append(float a, float b) + void append(double a, double b) { - float upper = std::max(a, b); - float lower = std::min(a, b); + double upper = std::max(a, b); + double lower = std::min(a, b); uppers.emplace_back(upper); lowers.emplace_back(lower); @@ -41,10 +41,10 @@ class Interval } } - void erase(float a, float b) + void erase(double a, double b) { - float upper = std::max(a, b); - float lower = std::min(a, b); + double upper = std::max(a, b); + double lower = std::min(a, b); for (size_t i = 0; i < uppers.size(); i++) { //完全消去 if (uppers[i] < upper && lowers[i] > lower) { @@ -74,22 +74,22 @@ class Interval std::sort(lowers.begin(), lowers.end()); } - float getWidth() + double getWidth() { - float width = 0.f; + double width = 0.f; for (size_t i = 0; i < lowers.size(); i++) { width += uppers[i] - lowers[i]; } return width; } - auto getLargestInterval() -> std::pair + auto getLargestInterval() -> std::pair { - float max_width = 0.f; - float max_lower = 0.f; - float max_upper = 0.f; + double max_width = 0.f; + double max_lower = 0.f; + double max_upper = 0.f; for (size_t i = 0; i < lowers.size(); i++) { - float width = uppers[i] - lowers[i]; + double width = uppers[i] - lowers[i]; if (width > max_width) { max_width = width; max_lower = lowers[i]; diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/field_analysis_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/field_analysis_wrapper.hpp index 0c1ca44f3..ba99edade 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/field_analysis_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/field_analysis_wrapper.hpp @@ -14,7 +14,7 @@ struct FieldAnalysisWrapper { std::string name; float unit; - Eigen::Vector2f origin; + Eigen::Vector2d origin; int size_x; int size_y; diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp index 0e62dccab..ff7c483e6 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp @@ -29,7 +29,7 @@ struct PlaySituationWrapper return situation_command.id == crane_msgs::msg::PlaySituation::INPLAY; } - Eigen::Vector2f placement_position; + Eigen::Vector2d placement_position; auto update(const crane_msgs::msg::PlaySituation & msg) -> void; From 215d40fb1e2f8925377ecb7b698e5620cdb143b8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 3 Dec 2023 23:03:35 +0900 Subject: [PATCH 06/15] =?UTF-8?q?=E9=9D=99=E7=9A=84=E5=9B=9E=E9=81=BF?= =?UTF-8?q?=E3=83=9E=E3=83=83=E3=83=97=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_local_planner/local_planner.hpp | 4 ++ .../src/crane_local_planner.cpp | 40 +++++++++++++++++++ .../world_model_wrapper.hpp | 12 ++++++ 3 files changed, 56 insertions(+) diff --git a/crane_local_planner/include/crane_local_planner/local_planner.hpp b/crane_local_planner/include/crane_local_planner/local_planner.hpp index 018eba143..21f6425cc 100644 --- a/crane_local_planner/include/crane_local_planner/local_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/local_planner.hpp @@ -89,6 +89,8 @@ class LocalPlannerComponent : public rclcpp::Node world_model = std::make_shared(*this); + world_model->addCallback([this]() { updateAvoidanceMapOnWorldModel(); }); + // TODO(HansRobo): add goal area as obstacles // TODO(HansRobo): add external area as obstacles @@ -112,6 +114,8 @@ class LocalPlannerComponent : public rclcpp::Node void callbackControlTarget(const crane_msgs::msg::RobotCommands &); + void updateAvoidanceMapOnWorldModel(); + private: rclcpp::Subscription::SharedPtr control_targets_sub; diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp index e9ae64516..a159635e4 100644 --- a/crane_local_planner/src/crane_local_planner.cpp +++ b/crane_local_planner/src/crane_local_planner.cpp @@ -254,6 +254,46 @@ void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCo commnads_pub->publish(commands); } } + +void LocalPlannerComponent::updateAvoidanceMapOnWorldModel() +{ + // update map size + + if ( + map.getLength().x() != world_model->field_size.x() || + map.getLength().y() != world_model->field_size.y()) { + map.clearAll(); + map.setGeometry( + grid_map::Length(world_model->field_size.x(), world_model->field_size.y()), MAP_RESOLUTION); + } + + // DefenseSize更新時にdefense_areaを更新する + static Vector2 defense_area_size; + if ( + defense_area_size.x() != world_model->defense_area_size.x() || + defense_area_size.y() != world_model->defense_area_size.y()) { + defense_area_size = world_model->defense_area_size; + if (not map.exists("defense_area")) { + map.add("defense_area"); + } + + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + grid_map::Position position; + map.getPosition(*iterator, position); + map.at("defense_area", *iterator) = world_model->isDefenseArea(position); + } + } + + // ボールプレイスメントMap + if (not map.exists("ball_placement")) { + map.add("ball_placement"); + } + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + grid_map::Position position; + map.getPosition(*iterator, position); + map.at("ball_placement", *iterator) = world_model->isBallPlacementArea(position); + } +} } // namespace crane #include diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp index 8de8eb555..bd9f3796a 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp @@ -369,6 +369,18 @@ struct WorldModelWrapper return isInRect(field_rect, p); } + bool isBallPlacementArea(Point p) const + { + // During ball placement, all robots of the non-placing team have to keep + // at least 0.5 meters distance to the line between the ball and the placement position + // (the forbidden area forms a stadium shape). + // ref: https://robocup-ssl.github.io/ssl-rules/sslrules.html#_ball_placement_interference + // Segment ball_placement_line; + // {Point(ball_placement_target), Point(ball.pos)}; + Segment ball_placement_line(ball_placement_target, ball.pos); + return bg::distance(ball_placement_line, p) <= 0.5; + } + double getDefenseWidth() const { return ours.defense_area.max.y() - ours.defense_area.min.y(); } double getDefenseHeight() const { return ours.defense_area.max.x() - ours.defense_area.min.x(); } From 71fd8f0ef84e85eaf234566d59d56c437850ffff Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 3 Dec 2023 23:31:43 +0900 Subject: [PATCH 07/15] =?UTF-8?q?=E5=8B=95=E7=9A=84=E5=9B=9E=E9=81=BF?= =?UTF-8?q?=E3=83=9E=E3=83=83=E3=83=97=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/crane_local_planner.cpp | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp index a159635e4..eb9a1053f 100644 --- a/crane_local_planner/src/crane_local_planner.cpp +++ b/crane_local_planner/src/crane_local_planner.cpp @@ -293,6 +293,58 @@ void LocalPlannerComponent::updateAvoidanceMapOnWorldModel() map.getPosition(*iterator, position); map.at("ball_placement", *iterator) = world_model->isBallPlacementArea(position); } + + // 味方ロボットMap + if (not map.exists("friend_robot")) { + map.add("friend_robot"); + } + map["friend_robot"].setConstant(0.0); + for (const auto & robot : world_model->ours.getAvailableRobots()) { + for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); + ++iterator) { + map.at("friend_robot", *iterator) = 1.0; + } + } + + // 敵ロボットMap + if (not map.exists("enemy_robot")) { + map.add("enemy_robot"); + } + map["enemy_robot"].setConstant(0.0); + for (const auto & robot : world_model->theirs.getAvailableRobots()) { + for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); + ++iterator) { + map.at("enemy_robot", *iterator) = 1.0; + } + } + + // ボールMap + if (not map.exists("ball")) { + map.add("ball"); + } + map["ball"].setConstant(0.0); + for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.1); !iterator.isPastEnd(); + ++iterator) { + map.at("ball", *iterator) = 1.0; + } + + // ボールMap (時間) + if (not map.exists("ball_time")) { + map.add("ball_time"); + } + Vector2 ball_vel_unit = world_model->ball.vel.normalized() * MAP_RESOLUTION; + Point ball_pos = world_model->ball.pos; + double time = 0.0; + const double TIME_STEP = MAP_RESOLUTION / world_model->ball.vel.norm(); + map["ball_time"].setConstant(100.0); + for (int i = 0; i < 100; ++i) { + for (grid_map::CircleIterator iterator(map, ball_pos, 0.05); !iterator.isPastEnd(); + ++iterator) { + map.at("ball_time", *iterator) = std::min(map.at("ball_time", *iterator), (float)time); + } + ball_pos += ball_vel_unit; + time += TIME_STEP; + } } } // namespace crane From 129c0e862d300d70da59162a6ae39cbe744355b1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 4 Dec 2023 01:11:42 +0900 Subject: [PATCH 08/15] =?UTF-8?q?=E5=9B=9E=E9=81=BF=E3=83=9E=E3=83=83?= =?UTF-8?q?=E3=83=97=E3=82=92publish(Visualize=E7=94=A8)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_local_planner/src/crane_local_planner.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp index eb9a1053f..bed75a0d3 100644 --- a/crane_local_planner/src/crane_local_planner.cpp +++ b/crane_local_planner/src/crane_local_planner.cpp @@ -345,6 +345,12 @@ void LocalPlannerComponent::updateAvoidanceMapOnWorldModel() ball_pos += ball_vel_unit; time += TIME_STEP; } + map.setTimestamp(now().nanoseconds()); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(map); + static auto publisher = + create_publisher("local_planner/grid_map", 1); + publisher->publish(std::move(message)); } } // namespace crane From 8c82ccede86e4ba6a5ae2ecaee125d0575de552f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 9 Dec 2023 17:16:04 +0900 Subject: [PATCH 09/15] =?UTF-8?q?=E3=83=97=E3=83=A9=E3=83=B3=E3=83=8A?= =?UTF-8?q?=E3=82=92=E3=82=A2=E3=83=AB=E3=82=B4=E3=83=AA=E3=82=BA=E3=83=A0?= =?UTF-8?q?=E3=81=94=E3=81=A8=E3=81=AB=E3=83=95=E3=82=A1=E3=82=A4=E3=83=AB?= =?UTF-8?q?=E3=82=92=E5=88=86=E5=89=B2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_local_planner/gridmap_planner.hpp | 130 +++++++ .../crane_local_planner/local_planner.hpp | 121 ++----- .../crane_local_planner/rvo_planner.hpp | 239 +++++++++++++ .../crane_local_planner/simple_planner.hpp | 118 ++++++ .../src/crane_local_planner.cpp | 338 +----------------- 5 files changed, 516 insertions(+), 430 deletions(-) create mode 100644 crane_local_planner/include/crane_local_planner/gridmap_planner.hpp create mode 100644 crane_local_planner/include/crane_local_planner/rvo_planner.hpp create mode 100644 crane_local_planner/include/crane_local_planner/simple_planner.hpp diff --git a/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp b/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp new file mode 100644 index 000000000..ff1d4587b --- /dev/null +++ b/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp @@ -0,0 +1,130 @@ +// Copyright (c) 2023 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#ifndef CRANE_GRIDMAP_PLANNER_HPP +#define CRANE_GRIDMAP_PLANNER_HPP + +#include +#include +#include + +class GridMapPlanner { + public: + GridMapPlanner(rclcpp::Node & node){ + node.declare_parameter("map_resolution", MAP_RESOLUTION); + MAP_RESOLUTION = get_parameter("map_resolution").as_double(); + + node.gridmap_publisher = + create_publisher("local_planner/grid_map", 1); + map.setFrameId("map"); + map.setGeometry(grid_map::Length(1.2, 2.0), MAP_RESOLUTION, grid_map::Position(0.0, 0.0)); + } + + crane_msgs::msg::RobotCommands calculateControlTarget(const crane_msgs::msg::RobotCommands &, WorldModelWrapper::SharedPtr world_model){ + // update map size + + if ( + map.getLength().x() != world_model->field_size.x() || + map.getLength().y() != world_model->field_size.y()) { + map.clearAll(); + map.setGeometry( + grid_map::Length(world_model->field_size.x(), world_model->field_size.y()), MAP_RESOLUTION); + } + + // DefenseSize更新時にdefense_areaを更新する + static Vector2 defense_area_size; + if ( + defense_area_size.x() != world_model->defense_area_size.x() || + defense_area_size.y() != world_model->defense_area_size.y()) { + defense_area_size = world_model->defense_area_size; + if (not map.exists("defense_area")) { + map.add("defense_area"); + } + + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + grid_map::Position position; + map.getPosition(*iterator, position); + map.at("defense_area", *iterator) = world_model->isDefenseArea(position); + } + } + + // ボールプレイスメントMap + if (not map.exists("ball_placement")) { + map.add("ball_placement"); + } + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + grid_map::Position position; + map.getPosition(*iterator, position); + map.at("ball_placement", *iterator) = world_model->isBallPlacementArea(position); + } + + // 味方ロボットMap + if (not map.exists("friend_robot")) { + map.add("friend_robot"); + } + map["friend_robot"].setConstant(0.0); + for (const auto & robot : world_model->ours.getAvailableRobots()) { + for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); + ++iterator) { + map.at("friend_robot", *iterator) = 1.0; + } + } + + // 敵ロボットMap + if (not map.exists("enemy_robot")) { + map.add("enemy_robot"); + } + map["enemy_robot"].setConstant(0.0); + for (const auto & robot : world_model->theirs.getAvailableRobots()) { + for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); + ++iterator) { + map.at("enemy_robot", *iterator) = 1.0; + } + } + + // ボールMap + if (not map.exists("ball")) { + map.add("ball"); + } + map["ball"].setConstant(0.0); + for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.1); !iterator.isPastEnd(); + ++iterator) { + map.at("ball", *iterator) = 1.0; + } + + // ボールMap (時間) + if (not map.exists("ball_time")) { + map.add("ball_time"); + } + Vector2 ball_vel_unit = world_model->ball.vel.normalized() * MAP_RESOLUTION; + Point ball_pos = world_model->ball.pos; + double time = 0.0; + const double TIME_STEP = MAP_RESOLUTION / world_model->ball.vel.norm(); + map["ball_time"].setConstant(100.0); + for (int i = 0; i < 100; ++i) { + for (grid_map::CircleIterator iterator(map, ball_pos, 0.05); !iterator.isPastEnd(); + ++iterator) { + map.at("ball_time", *iterator) = std::min(map.at("ball_time", *iterator), (float)time); + } + ball_pos += ball_vel_unit; + time += TIME_STEP; + } + map.setTimestamp(now().nanoseconds()); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(map); + + gridmap_publisher->publish(std::move(message)); + } + + private: + rclcpp::Publisher::SharedPtr gridmap_publisher; + + grid_map::GridMap map; + + double MAP_RESOLUTION = 0.05; + +} +#endif //CRANE_GRIDMAP_PLANNER_HPP diff --git a/crane_local_planner/include/crane_local_planner/local_planner.hpp b/crane_local_planner/include/crane_local_planner/local_planner.hpp index 21f6425cc..78682ef1d 100644 --- a/crane_local_planner/include/crane_local_planner/local_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/local_planner.hpp @@ -7,7 +7,7 @@ #ifndef CRANE_LOCAL_PLANNER__LOCAL_PLANNER_HPP_ #define CRANE_LOCAL_PLANNER__LOCAL_PLANNER_HPP_ -#include + #include #include #include @@ -16,7 +16,9 @@ #include #include -#include "RVO.h" +#include "simple_planner.hpp" +#include "rvo_planner.hpp" +#include "gridmap_planner.hpp" #include "visibility_control.h" namespace crane @@ -36,82 +38,36 @@ class LocalPlannerComponent : public rclcpp::Node : rclcpp::Node("local_planner", options), map({"penalty", "ball_placement", "theirs", "ours", "ball"}) { - declare_parameter("enable_rvo", true); - enable_rvo = get_parameter("enable_rvo").as_bool(); - - declare_parameter("rvo_time_step", RVO_TIME_STEP); - RVO_TIME_STEP = get_parameter("rvo_time_step").as_double(); - declare_parameter("rvo_neighbor_dist", RVO_NEIGHBOR_DIST); - RVO_NEIGHBOR_DIST = get_parameter("rvo_neighbor_dist").as_double(); - declare_parameter("rvo_max_neighbors", RVO_MAX_NEIGHBORS); - RVO_MAX_NEIGHBORS = get_parameter("rvo_max_neighbors").as_int(); - declare_parameter("rvo_time_horizon", RVO_TIME_HORIZON); - RVO_TIME_HORIZON = get_parameter("rvo_time_horizon").as_double(); - declare_parameter("rvo_time_horizon_obst", RVO_TIME_HORIZON_OBST); - RVO_TIME_HORIZON_OBST = get_parameter("rvo_time_horizon_obst").as_double(); - declare_parameter("rvo_radius", RVO_RADIUS); - RVO_RADIUS = get_parameter("rvo_radius").as_double(); - declare_parameter("rvo_max_speed", RVO_MAX_SPEED); - RVO_MAX_SPEED = get_parameter("rvo_max_speed").as_double(); - declare_parameter("rvo_trapezoidal_max_acc", RVO_TRAPEZOIDAL_MAX_ACC); - RVO_TRAPEZOIDAL_MAX_ACC = get_parameter("rvo_trapezoidal_max_acc").as_double(); - declare_parameter("rvo_trapezoidal_frame_rate", RVO_TRAPEZOIDAL_FRAME_RATE); - RVO_TRAPEZOIDAL_FRAME_RATE = get_parameter("rvo_trapezoidal_frame_rate").as_double(); - declare_parameter("rvo_trapezoidal_max_speed", RVO_TRAPEZOIDAL_MAX_SPEED); - RVO_TRAPEZOIDAL_MAX_SPEED = get_parameter("rvo_trapezoidal_max_speed").as_double(); - declare_parameter("non_rvo_max_vel", NON_RVO_MAX_VEL); - NON_RVO_MAX_VEL = get_parameter("non_rvo_max_vel").as_double(); - - declare_parameter("non_rvo_p_gain", NON_RVO_P_GAIN); - NON_RVO_P_GAIN = get_parameter("non_rvo_p_gain").as_double(); - declare_parameter("non_rvo_i_gain", NON_RVO_I_GAIN); - NON_RVO_I_GAIN = get_parameter("non_rvo_i_gain").as_double(); - declare_parameter("non_rvo_d_gain", NON_RVO_D_GAIN); - NON_RVO_D_GAIN = get_parameter("non_rvo_d_gain").as_double(); - - for (auto & controller : vx_controllers) { - controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN); - } - - for (auto & controller : vy_controllers) { - controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN); + declare_parameter("planner", "simple"); + auto planner_str = get_parameter("enable_rvo").as_string(); + + if(planner_str == "simple"){ + simple_planner = std::make_shared(*this); + calculate_control_target = [this](const crane_msgs::msg::RobotCommands & msg){ + return simple_planner->calculateControlTarget(msg,world_model); + }; + }else if(planner_str == "rvo"){ + rvo_planner = std::make_shared(*this); + calculate_control_target = [this](const crane_msgs::msg::RobotCommands & msg){ + return rvo_planner->calculateControlTarget(msg,world_model); + }; + }else if(planner_str == "gridmap"){ + gridmap_planner = std::make_shared(*this); + calculate_control_target = [this](const crane_msgs::msg::RobotCommands & msg){ + return gridmap_planner->calculateControlTarget(msg,world_model); + }; } - rvo_sim = std::make_unique( - RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST, - RVO_RADIUS, RVO_MAX_SPEED); - - // MAP_RESOLUTION = get_parameter("map_resolution").as_double(); - declare_parameter("map_resolution", MAP_RESOLUTION); - MAP_RESOLUTION = get_parameter("map_resolution").as_double(); - map.setFrameId("map"); - map.setGeometry(grid_map::Length(1.2, 2.0), MAP_RESOLUTION, grid_map::Position(0.0, 0.0)); - world_model = std::make_shared(*this); world_model->addCallback([this]() { updateAvoidanceMapOnWorldModel(); }); - // TODO(HansRobo): add goal area as obstacles - - // TODO(HansRobo): add external area as obstacles - // friend robots -> 0~19 - // enemy robots -> 20~39 - // ball 40 - for (int i = 0; i < 41; i++) { - rvo_sim->addAgent(RVO::Vector2(20.0f, 20.0f)); - } - - commnads_pub = this->create_publisher("/robot_commands", 10); + commands_pub = this->create_publisher("/robot_commands", 10); control_targets_sub = this->create_subscription( "/control_targets", 10, std::bind(&LocalPlannerComponent::callbackControlTarget, this, std::placeholders::_1)); } - void reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands &); - - crane_msgs::msg::RobotCommands extractRobotCommandsFromRVOSim( - const crane_msgs::msg::RobotCommands &); - void callbackControlTarget(const crane_msgs::msg::RobotCommands &); void updateAvoidanceMapOnWorldModel(); @@ -119,37 +75,16 @@ class LocalPlannerComponent : public rclcpp::Node private: rclcpp::Subscription::SharedPtr control_targets_sub; - rclcpp::Publisher::SharedPtr commnads_pub; - - std::unique_ptr rvo_sim; + rclcpp::Publisher::SharedPtr commands_pub; WorldModelWrapper::SharedPtr world_model; - bool enable_rvo; - - std::array vx_controllers; - std::array vy_controllers; - - float RVO_TIME_STEP = 1.0 / 60.0f; - float RVO_NEIGHBOR_DIST = 2.0f; - int RVO_MAX_NEIGHBORS = 5; - float RVO_TIME_HORIZON = 1.f; - float RVO_TIME_HORIZON_OBST = 1.f; - float RVO_RADIUS = 0.09f; - float RVO_MAX_SPEED = 10.0f; - - float RVO_TRAPEZOIDAL_MAX_ACC = 8.0; - float RVO_TRAPEZOIDAL_FRAME_RATE = 60; - float RVO_TRAPEZOIDAL_MAX_SPEED = 4.0; - - double NON_RVO_MAX_VEL = 4.0; - double NON_RVO_P_GAIN = 4.0; - double NON_RVO_I_GAIN = 0.0; - double NON_RVO_D_GAIN = 0.0; + std::function calculate_control_target; - grid_map::GridMap map; + std::shared_ptr simple_planner = nullptr; + std::shared_ptr rvo_planner = nullptr; + std::shared_ptr gridmap_planner = nullptr; - double MAP_RESOLUTION = 0.05; }; } // namespace crane diff --git a/crane_local_planner/include/crane_local_planner/rvo_planner.hpp b/crane_local_planner/include/crane_local_planner/rvo_planner.hpp new file mode 100644 index 000000000..400244631 --- /dev/null +++ b/crane_local_planner/include/crane_local_planner/rvo_planner.hpp @@ -0,0 +1,239 @@ +// Copyright (c) 2022 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#ifndef CRANE_RVO_PLANNER_HPP +#define CRANE_RVO_PLANNER_HPP + +#include "RVO.h" +#include +#include + +class RVOPlanner{ +public: + RVOPlanner(rclcpp::Node & node){ + + node.declare_parameter("rvo_time_step", RVO_TIME_STEP); + RVO_TIME_STEP = get_parameter("rvo_time_step").as_double(); + node.declare_parameter("rvo_neighbor_dist", RVO_NEIGHBOR_DIST); + RVO_NEIGHBOR_DIST = get_parameter("rvo_neighbor_dist").as_double(); + node.declare_parameter("rvo_max_neighbors", RVO_MAX_NEIGHBORS); + RVO_MAX_NEIGHBORS = get_parameter("rvo_max_neighbors").as_int(); + node.declare_parameter("rvo_time_horizon", RVO_TIME_HORIZON); + RVO_TIME_HORIZON = get_parameter("rvo_time_horizon").as_double(); + node.declare_parameter("rvo_time_horizon_obst", RVO_TIME_HORIZON_OBST); + RVO_TIME_HORIZON_OBST = get_parameter("rvo_time_horizon_obst").as_double(); + node.declare_parameter("rvo_radius", RVO_RADIUS); + RVO_RADIUS = get_parameter("rvo_radius").as_double(); + node.declare_parameter("rvo_max_speed", RVO_MAX_SPEED); + RVO_MAX_SPEED = get_parameter("rvo_max_speed").as_double(); + node.declare_parameter("rvo_trapezoidal_max_acc", RVO_TRAPEZOIDAL_MAX_ACC); + RVO_TRAPEZOIDAL_MAX_ACC = get_parameter("rvo_trapezoidal_max_acc").as_double(); + node.declare_parameter("rvo_trapezoidal_frame_rate", RVO_TRAPEZOIDAL_FRAME_RATE); + RVO_TRAPEZOIDAL_FRAME_RATE = get_parameter("rvo_trapezoidal_frame_rate").as_double(); + node.declare_parameter("rvo_trapezoidal_max_speed", RVO_TRAPEZOIDAL_MAX_SPEED); + RVO_TRAPEZOIDAL_MAX_SPEED = get_parameter("rvo_trapezoidal_max_speed").as_double(); + + rvo_sim = std::make_unique( + RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST, + RVO_RADIUS, RVO_MAX_SPEED); + + // TODO(HansRobo): add goal area as obstacles + + // TODO(HansRobo): add external area as obstacles + // friend robots -> 0~19 + // enemy robots -> 20~39 + // ball 40 + for (int i = 0; i < 41; i++) { + rvo_sim->addAgent(RVO::Vector2(20.0f, 20.0f)); + } + } + + void reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands &) + { + bool add_ball = true; + // 味方ロボット：RVO内の位置・速度（＝進みたい方向）の更新 + for (const auto & command : msg.robot_commands) { + rvo_sim->setAgentPosition( + command.robot_id, RVO::Vector2(command.current_pose.x, command.current_pose.y)); + rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(0.f, 0.f)); + + auto robot = world_model->getOurRobot(command.robot_id); + if (robot->available && command.local_planner_config.disable_collision_avoidance) { + add_ball = false; + } + + float target_x = command.target_x.front(); + float target_y = command.target_y.front(); + + if (command.motion_mode_enable) { + // 速度制御モードの場合：速度司令をそのままRVOのpreferred velocityとして設定する + rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(target_x, target_y)); + } else { + // 位置制御モードの場合：目標位置方向に移動する速度ベクトルをRVOのpreferred velocityとして設定する + const auto pos = robot->pose.pos; + auto diff_pos = Point(target_x - pos.x(), target_y - pos.y()); + + // 台形加速制御 + // 2ax = v^2 - v0^2 + // v^2 - 2ax = v0^2 + // v0 = sqrt(v^2 - 2ax) + double max_speed_for_stop = + std::sqrt(0 * 0 - 2.0 * (-RVO_TRAPEZOIDAL_MAX_ACC) * diff_pos.norm()); + double max_speed_for_acc = + robot->vel.linear.norm() + RVO_TRAPEZOIDAL_MAX_ACC / RVO_TRAPEZOIDAL_FRAME_RATE; + + double target_speed = + std::min({max_speed_for_acc, max_speed_for_stop, (double)RVO_TRAPEZOIDAL_MAX_SPEED}); + Velocity vel; + double dist = diff_pos.norm(); + vel << target_speed / dist * diff_pos.x(), target_speed / dist * diff_pos.y(); + + if (command.robot_id == 3) { + std::cout << "current_robot: " << int(command.robot_id) << std::endl; + std::cout << "from: " << pos.x() << ", " << pos.y() << std::endl; + std::cout << "to: " << target_x << ", " << target_y << std::endl; + std::cout << "diff_pos: " << diff_pos.x() << ", " << diff_pos.y() << std::endl; + std::cout << "target_speed: " << target_speed << std::endl; + std::cout << "target_vel: " << std::fixed << std::setprecision(5) << vel.x() << ", " + << vel.y() << std::endl; + } + rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(vel.x(), vel.y())); + } + } + + if (add_ball) { + rvo_sim->setAgentPosition( + 40, RVO::Vector2(world_model->ball.pos.x(), world_model->ball.pos.y())); + rvo_sim->setAgentPrefVelocity(40, RVO::Vector2(0.f, 0.f)); + } else { + rvo_sim->setAgentPosition(40, RVO::Vector2(20.0f, 20.0f)); + rvo_sim->setAgentPrefVelocity(40, RVO::Vector2(0.f, 0.f)); + } + + // for (const auto & friend_robot : world_model->ours.robots) { + // if (not friend_robot->available) { + // rvo_sim->setAgentPosition(friend_robot->id, RVO::Vector2(20.f, 20.f + friend_robot->id)); + // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(0.f, 0.f)); + // + // } else { + // // Visionからのロボット位置の更新 + // const auto & pos = friend_robot->pose.pos; + // rvo_sim->setAgentPosition(friend_robot->id, RVO::Vector2(pos.x(), pos.y())); + // + // auto robot_target = std::find_if( + // msg.robot_commands.begin(), msg.robot_commands.end(), + // [&](const auto & x) { return x.robot_id == friend_robot->id; }); + // + // if (robot_target == msg.robot_commands.end()) { + // // ロボットがcontrol_targetsに含まれていない場合、 + // // 観測された速度をpreferred velocityとして設定する + // rvo_sim->setAgentPrefVelocity( + // friend_robot->id, + // RVO::Vector2(friend_robot->vel.linear.x(), friend_robot->vel.linear.y())); + // } else { + // float target_x = robot_target->target_x.front(); + // float target_y = robot_target->target_y.front(); + // if (robot_target->motion_mode_enable) { + // // 速度制御モードの場合：速度司令をそのままRVOのpreferred velocityとして設定する + // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(target_x, target_y)); + // } else { + // // 位置制御モードの場合：目標位置方向に移動する速度ベクトルをRVOのpreferred velocityとして設定する + // auto diff_pos = Point(target_x, target_y) - pos; + // + // // 台形加速制御 + // // TODO : 外部からパラメータを設定できるようにする + // constexpr double MAX_ACC = 10.0; + // constexpr double FRAME_RATE = 300; + // constexpr double MAX_SPEED = 10.0; + // std::cout << "current_robot: " << int(robot_target->robot_id) << std::endl; + // std::cout << "from: " << pos.x() << ", " << pos.y() << std::endl; + // std::cout << "to: " << target_x << ", " << target_y << std::endl; + // // 2ax = v^2 - v0^2 + // // v^2 - 2ax = v0^2 + // // v0 = sqrt(v^2 - 2ax) + // double max_speed_for_stop = std::sqrt(0 * 0 - 2.0 * (-MAX_ACC) * diff_pos.norm()); + // double max_speed_for_acc = friend_robot->vel.linear.norm() + MAX_ACC / FRAME_RATE; + // + // double target_speed = -std::min({max_speed_for_acc, max_speed_for_stop, MAX_SPEED}); + // auto vel = diff_pos * target_speed / diff_pos.norm(); + // std::cout << "target_vel: " << vel.x() << ", " << vel.y() << std::endl; + // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(vel.x(), vel.y())); + // } + // } + // } + // } + + for (const auto & enemy_robot : world_model->theirs.robots) { + if (enemy_robot->available) { + const auto & pos = enemy_robot->pose.pos; + const auto & vel = enemy_robot->vel.linear; + rvo_sim->setAgentPosition(enemy_robot->id + 20, RVO::Vector2(pos.x(), pos.y())); + rvo_sim->setAgentPrefVelocity(enemy_robot->id + 20, RVO::Vector2(vel.x(), vel.y())); + } else { + rvo_sim->setAgentPosition(enemy_robot->id + 20, RVO::Vector2(20.f, 20.f)); + rvo_sim->setAgentPrefVelocity(enemy_robot->id + 20, RVO::Vector2(0.f, 0.f)); + } + } + } + + crane_msgs::msg::RobotCommands extractRobotCommandsFromRVOSim( + const crane_msgs::msg::RobotCommands & msg) + { + crane_msgs::msg::RobotCommands commands = msg; + for (size_t i = 0; i < msg.robot_commands.size(); i++) { + const auto & original_command = msg.robot_commands.at(i); + const auto & robot = world_model->getOurRobot(original_command.robot_id); + crane_msgs::msg::RobotCommand command = original_command; + command.current_pose.x = robot->pose.pos.x(); + command.current_pose.y = robot->pose.pos.y(); + command.current_pose.theta = robot->pose.theta; + // RVOシミュレータの出力をコピーする + // NOTE: RVOシミュレータは角度を扱わないので角度はそのまま + + // RVOシミュレータの出力は速度なので，速度制御モードにする + command.motion_mode_enable = true; + auto vel = rvo_sim->getAgentVelocity(original_command.robot_id); + if (original_command.robot_id == 3) { + // std::cout << "robot_id " << int(original_command.robot_id) << std::endl; + // std::cout << "vel : " << vel.x() << " " << vel.y() << std::endl; + } + command.target_velocity.x = vel.x(); + command.target_velocity.y = vel.y(); + command.target_x.clear(); + command.target_y.clear(); + + commands.robot_commands.at(i) = command; + } + return commands; + } + + crane_msgs::msg::RobotCommands calculateControlTarget(const crane_msgs::msg::RobotCommands &, WorldModelWrapper::SharedPtr world_model){ + if (!world_model->hasUpdated()) { + return; + } + reflectWorldToRVOSim(msg); + // RVOシミュレータ更新 + rvo_sim->doStep(); + return extractRobotCommandsFromRVOSim(msg); + } + +private: + std::unique_ptr rvo_sim; + + float RVO_TIME_STEP = 1.0 / 60.0f; + float RVO_NEIGHBOR_DIST = 2.0f; + int RVO_MAX_NEIGHBORS = 5; + float RVO_TIME_HORIZON = 1.f; + float RVO_TIME_HORIZON_OBST = 1.f; + float RVO_RADIUS = 0.09f; + float RVO_MAX_SPEED = 10.0f; + + float RVO_TRAPEZOIDAL_MAX_ACC = 8.0; + float RVO_TRAPEZOIDAL_FRAME_RATE = 60; + float RVO_TRAPEZOIDAL_MAX_SPEED = 4.0; +}; + +#endif //CRANE_RVO_PLANNER_HPP diff --git a/crane_local_planner/include/crane_local_planner/simple_planner.hpp b/crane_local_planner/include/crane_local_planner/simple_planner.hpp new file mode 100644 index 000000000..14527d888 --- /dev/null +++ b/crane_local_planner/include/crane_local_planner/simple_planner.hpp @@ -0,0 +1,118 @@ +// Copyright (c) 2023 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#ifndef CRANE_SIMPLE_PLANNER_HPP +#define CRANE_SIMPLE_PLANNER_HPP + +#include +#include +#include + +class SimplePlanner{ +public: + SimplePlanner(rclcpp::Node & node){ + node.declare_parameter("non_rvo_max_vel", NON_RVO_MAX_VEL); + NON_RVO_MAX_VEL = get_parameter("non_rvo_max_vel").as_double(); + + node.declare_parameter("non_rvo_p_gain", NON_RVO_P_GAIN); + NON_RVO_P_GAIN = get_parameter("non_rvo_p_gain").as_double(); + node.declare_parameter("non_rvo_i_gain", NON_RVO_I_GAIN); + NON_RVO_I_GAIN = get_parameter("non_rvo_i_gain").as_double(); + node.declare_parameter("non_rvo_d_gain", NON_RVO_D_GAIN); + NON_RVO_D_GAIN = get_parameter("non_rvo_d_gain").as_double(); + + for (auto & controller : vx_controllers) { + controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN); + } + + for (auto & controller : vy_controllers) { + controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN); + } + } + + crane_msgs::msg::RobotCommands calculateControlTarget(const crane_msgs::msg::RobotCommands & msg, WorldModelWrapper::SharedPtr world_model){ + crane_msgs::msg::RobotCommands commands = msg; + for (auto & command : commands.robot_commands) { + if ((not command.target_x.empty()) && (not command.target_y.empty())) { + auto robot = world_model->getOurRobot(command.robot_id); + Point target; + target << command.target_x.front(), command.target_y.front(); + + Point robot_to_target = target - robot->pose.pos; + + // double dot1 = (world_model->ball.pos - robot->pose.pos).dot(robot_to_target); + // double dot2 = (-robot_to_target).dot(world_model->ball.pos - target); + // if (dot1 > 0 && dot2 > 0) { + // Point norm_vec; + // norm_vec << robot_to_target.y(), -robot_to_target.x(); + // norm_vec = norm_vec.normalized(); + // double dist_to_line = std::abs(norm_vec.dot(world_model->ball.pos - robot->pose.pos)); + // if (dist_to_line < 0.1) { + // Point p1, p2; + // Point ball_est = world_model->ball.pos + world_model->ball.vel * 4.0; + // p1 = ball_est + 0.2 * norm_vec; + // p2 = ball_est - 0.2 * norm_vec; + // double d1 = (robot->pose.pos - p1).squaredNorm(); + // double d2 = (robot->pose.pos - p2).squaredNorm(); + // target = (d1 < d2) ? p1 : p2; + // } + // command.target_x.front() = target.x(); + // command.target_y.front() = target.y(); + // } + + double max_vel = command.local_planner_config.max_velocity > 0 + ? command.local_planner_config.max_velocity + : NON_RVO_MAX_VEL; + // double max_acc = command.local_planner_config.max_acceleration > 0? command.local_planner_config.max_acceleration : NON_RVO_GAIN; + double max_omega = command.local_planner_config.max_omega > 0 + ? command.local_planner_config.max_omega + : 600.0 * M_PI / 180; + + // 速度に変換する + Velocity vel; + vel << vx_controllers[command.robot_id].update( + command.target_x.front() - command.current_pose.x, 1.f / 30.f), + vy_controllers[command.robot_id].update( + command.target_y.front() - command.current_pose.y, 1.f / 30.f); + vel += vel.normalized() * command.local_planner_config.terminal_velocity; + if (vel.norm() > max_vel) { + vel = vel.normalized() * max_vel; + } + + command.target_velocity.x = vel.x(); + command.target_velocity.y = vel.y(); + + //　2023/11/12 出力の目標角度制限をしたらVisionの遅れと相まってロボットが角度方向に発振したのでコメントアウトする + // そしてこの過ちを再びおかさぬようここに残しておく． R.I.P. + // double MAX_THETA_DIFF = max_omega / 30.0f; + // // 1フレームで変化するthetaの量が大きすぎると急に回転するので制限する + // if (not command.target_theta.empty()) { + // double theta_diff = + // getAngleDiff(command.target_theta.front(), command.current_pose.theta); + // if (std::fabs(theta_diff) > MAX_THETA_DIFF) { + // theta_diff = std::copysign(MAX_THETA_DIFF, theta_diff); + // } + // + // command.target_theta.front() = command.current_pose.theta + theta_diff; + // } + + command.current_ball_x = world_model->ball.pos.x(); + command.current_ball_y = world_model->ball.pos.y(); + } + } + return commands; + } +private: + std::array vx_controllers; + std::array vy_controllers; + + double NON_RVO_MAX_VEL = 4.0; + double NON_RVO_P_GAIN = 4.0; + double NON_RVO_I_GAIN = 0.0; + double NON_RVO_D_GAIN = 0.0; +}; + +#endif //CRANE_SIMPLE_PLANNER_HPP diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp index bed75a0d3..8cd63da0d 100644 --- a/crane_local_planner/src/crane_local_planner.cpp +++ b/crane_local_planner/src/crane_local_planner.cpp @@ -8,349 +8,13 @@ namespace crane { -void LocalPlannerComponent::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg) -{ - bool add_ball = true; - // 味方ロボット：RVO内の位置・速度（＝進みたい方向）の更新 - for (const auto & command : msg.robot_commands) { - rvo_sim->setAgentPosition( - command.robot_id, RVO::Vector2(command.current_pose.x, command.current_pose.y)); - rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(0.f, 0.f)); - - auto robot = world_model->getOurRobot(command.robot_id); - if (robot->available && command.local_planner_config.disable_collision_avoidance) { - add_ball = false; - } - - float target_x = command.target_x.front(); - float target_y = command.target_y.front(); - - if (command.motion_mode_enable) { - // 速度制御モードの場合：速度司令をそのままRVOのpreferred velocityとして設定する - rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(target_x, target_y)); - } else { - // 位置制御モードの場合：目標位置方向に移動する速度ベクトルをRVOのpreferred velocityとして設定する - const auto pos = robot->pose.pos; - auto diff_pos = Point(target_x - pos.x(), target_y - pos.y()); - - // 台形加速制御 - // 2ax = v^2 - v0^2 - // v^2 - 2ax = v0^2 - // v0 = sqrt(v^2 - 2ax) - double max_speed_for_stop = - std::sqrt(0 * 0 - 2.0 * (-RVO_TRAPEZOIDAL_MAX_ACC) * diff_pos.norm()); - double max_speed_for_acc = - robot->vel.linear.norm() + RVO_TRAPEZOIDAL_MAX_ACC / RVO_TRAPEZOIDAL_FRAME_RATE; - - double target_speed = - std::min({max_speed_for_acc, max_speed_for_stop, (double)RVO_TRAPEZOIDAL_MAX_SPEED}); - Velocity vel; - double dist = diff_pos.norm(); - vel << target_speed / dist * diff_pos.x(), target_speed / dist * diff_pos.y(); - - if (command.robot_id == 3) { - std::cout << "current_robot: " << int(command.robot_id) << std::endl; - std::cout << "from: " << pos.x() << ", " << pos.y() << std::endl; - std::cout << "to: " << target_x << ", " << target_y << std::endl; - std::cout << "diff_pos: " << diff_pos.x() << ", " << diff_pos.y() << std::endl; - std::cout << "target_speed: " << target_speed << std::endl; - std::cout << "target_vel: " << std::fixed << std::setprecision(5) << vel.x() << ", " - << vel.y() << std::endl; - } - rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(vel.x(), vel.y())); - } - } - - if (add_ball) { - rvo_sim->setAgentPosition( - 40, RVO::Vector2(world_model->ball.pos.x(), world_model->ball.pos.y())); - rvo_sim->setAgentPrefVelocity(40, RVO::Vector2(0.f, 0.f)); - } else { - rvo_sim->setAgentPosition(40, RVO::Vector2(20.0f, 20.0f)); - rvo_sim->setAgentPrefVelocity(40, RVO::Vector2(0.f, 0.f)); - } - - // for (const auto & friend_robot : world_model->ours.robots) { - // if (not friend_robot->available) { - // rvo_sim->setAgentPosition(friend_robot->id, RVO::Vector2(20.f, 20.f + friend_robot->id)); - // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(0.f, 0.f)); - // - // } else { - // // Visionからのロボット位置の更新 - // const auto & pos = friend_robot->pose.pos; - // rvo_sim->setAgentPosition(friend_robot->id, RVO::Vector2(pos.x(), pos.y())); - // - // auto robot_target = std::find_if( - // msg.robot_commands.begin(), msg.robot_commands.end(), - // [&](const auto & x) { return x.robot_id == friend_robot->id; }); - // - // if (robot_target == msg.robot_commands.end()) { - // // ロボットがcontrol_targetsに含まれていない場合、 - // // 観測された速度をpreferred velocityとして設定する - // rvo_sim->setAgentPrefVelocity( - // friend_robot->id, - // RVO::Vector2(friend_robot->vel.linear.x(), friend_robot->vel.linear.y())); - // } else { - // float target_x = robot_target->target_x.front(); - // float target_y = robot_target->target_y.front(); - // if (robot_target->motion_mode_enable) { - // // 速度制御モードの場合：速度司令をそのままRVOのpreferred velocityとして設定する - // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(target_x, target_y)); - // } else { - // // 位置制御モードの場合：目標位置方向に移動する速度ベクトルをRVOのpreferred velocityとして設定する - // auto diff_pos = Point(target_x, target_y) - pos; - // - // // 台形加速制御 - // // TODO : 外部からパラメータを設定できるようにする - // constexpr double MAX_ACC = 10.0; - // constexpr double FRAME_RATE = 300; - // constexpr double MAX_SPEED = 10.0; - // std::cout << "current_robot: " << int(robot_target->robot_id) << std::endl; - // std::cout << "from: " << pos.x() << ", " << pos.y() << std::endl; - // std::cout << "to: " << target_x << ", " << target_y << std::endl; - // // 2ax = v^2 - v0^2 - // // v^2 - 2ax = v0^2 - // // v0 = sqrt(v^2 - 2ax) - // double max_speed_for_stop = std::sqrt(0 * 0 - 2.0 * (-MAX_ACC) * diff_pos.norm()); - // double max_speed_for_acc = friend_robot->vel.linear.norm() + MAX_ACC / FRAME_RATE; - // - // double target_speed = -std::min({max_speed_for_acc, max_speed_for_stop, MAX_SPEED}); - // auto vel = diff_pos * target_speed / diff_pos.norm(); - // std::cout << "target_vel: " << vel.x() << ", " << vel.y() << std::endl; - // rvo_sim->setAgentPrefVelocity(friend_robot->id, RVO::Vector2(vel.x(), vel.y())); - // } - // } - // } - // } - - for (const auto & enemy_robot : world_model->theirs.robots) { - if (enemy_robot->available) { - const auto & pos = enemy_robot->pose.pos; - const auto & vel = enemy_robot->vel.linear; - rvo_sim->setAgentPosition(enemy_robot->id + 20, RVO::Vector2(pos.x(), pos.y())); - rvo_sim->setAgentPrefVelocity(enemy_robot->id + 20, RVO::Vector2(vel.x(), vel.y())); - } else { - rvo_sim->setAgentPosition(enemy_robot->id + 20, RVO::Vector2(20.f, 20.f)); - rvo_sim->setAgentPrefVelocity(enemy_robot->id + 20, RVO::Vector2(0.f, 0.f)); - } - } -} - -// RVOシミュレータの結果をコマンドにコピー -crane_msgs::msg::RobotCommands LocalPlannerComponent::extractRobotCommandsFromRVOSim( - const crane_msgs::msg::RobotCommands & msg) -{ - crane_msgs::msg::RobotCommands commands = msg; - for (size_t i = 0; i < msg.robot_commands.size(); i++) { - const auto & original_command = msg.robot_commands.at(i); - const auto & robot = world_model->getOurRobot(original_command.robot_id); - crane_msgs::msg::RobotCommand command = original_command; - command.current_pose.x = robot->pose.pos.x(); - command.current_pose.y = robot->pose.pos.y(); - command.current_pose.theta = robot->pose.theta; - // RVOシミュレータの出力をコピーする - // NOTE: RVOシミュレータは角度を扱わないので角度はそのまま - - // RVOシミュレータの出力は速度なので，速度制御モードにする - command.motion_mode_enable = true; - auto vel = rvo_sim->getAgentVelocity(original_command.robot_id); - if (original_command.robot_id == 3) { - // std::cout << "robot_id " << int(original_command.robot_id) << std::endl; - // std::cout << "vel : " << vel.x() << " " << vel.y() << std::endl; - } - command.target_velocity.x = vel.x(); - command.target_velocity.y = vel.y(); - command.target_x.clear(); - command.target_y.clear(); - - commands.robot_commands.at(i) = command; - } - return commands; -} - void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCommands & msg) { if (!world_model->hasUpdated()) { return; } - if (enable_rvo) { - reflectWorldToRVOSim(msg); - - // RVOシミュレータ更新 - rvo_sim->doStep(); - - commnads_pub->publish(extractRobotCommandsFromRVOSim(msg)); - } else { - crane_msgs::msg::RobotCommands commands = msg; - for (auto & command : commands.robot_commands) { - if ((not command.target_x.empty()) && (not command.target_y.empty())) { - auto robot = world_model->getOurRobot(command.robot_id); - Point target; - target << command.target_x.front(), command.target_y.front(); - - Point robot_to_target = target - robot->pose.pos; - - // double dot1 = (world_model->ball.pos - robot->pose.pos).dot(robot_to_target); - // double dot2 = (-robot_to_target).dot(world_model->ball.pos - target); - // if (dot1 > 0 && dot2 > 0) { - // Point norm_vec; - // norm_vec << robot_to_target.y(), -robot_to_target.x(); - // norm_vec = norm_vec.normalized(); - // double dist_to_line = std::abs(norm_vec.dot(world_model->ball.pos - robot->pose.pos)); - // if (dist_to_line < 0.1) { - // Point p1, p2; - // Point ball_est = world_model->ball.pos + world_model->ball.vel * 4.0; - // p1 = ball_est + 0.2 * norm_vec; - // p2 = ball_est - 0.2 * norm_vec; - // double d1 = (robot->pose.pos - p1).squaredNorm(); - // double d2 = (robot->pose.pos - p2).squaredNorm(); - // target = (d1 < d2) ? p1 : p2; - // } - // command.target_x.front() = target.x(); - // command.target_y.front() = target.y(); - // } - - double max_vel = command.local_planner_config.max_velocity > 0 - ? command.local_planner_config.max_velocity - : NON_RVO_MAX_VEL; - // double max_acc = command.local_planner_config.max_acceleration > 0? command.local_planner_config.max_acceleration : NON_RVO_GAIN; - double max_omega = command.local_planner_config.max_omega > 0 - ? command.local_planner_config.max_omega - : 600.0 * M_PI / 180; - - // 速度に変換する - Velocity vel; - vel << vx_controllers[command.robot_id].update( - command.target_x.front() - command.current_pose.x, 1.f / 30.f), - vy_controllers[command.robot_id].update( - command.target_y.front() - command.current_pose.y, 1.f / 30.f); - vel += vel.normalized() * command.local_planner_config.terminal_velocity; - if (vel.norm() > max_vel) { - vel = vel.normalized() * max_vel; - } - - command.target_velocity.x = vel.x(); - command.target_velocity.y = vel.y(); - - //　2023/11/12 出力の目標角度制限をしたらVisionの遅れと相まってロボットが角度方向に発振したのでコメントアウトする - // そしてこの過ちを再びおかさぬようここに残しておく． R.I.P. - // double MAX_THETA_DIFF = max_omega / 30.0f; - // // 1フレームで変化するthetaの量が大きすぎると急に回転するので制限する - // if (not command.target_theta.empty()) { - // double theta_diff = - // getAngleDiff(command.target_theta.front(), command.current_pose.theta); - // if (std::fabs(theta_diff) > MAX_THETA_DIFF) { - // theta_diff = std::copysign(MAX_THETA_DIFF, theta_diff); - // } - // - // command.target_theta.front() = command.current_pose.theta + theta_diff; - // } - - command.current_ball_x = world_model->ball.pos.x(); - command.current_ball_y = world_model->ball.pos.y(); - } - } - commnads_pub->publish(commands); - } -} - -void LocalPlannerComponent::updateAvoidanceMapOnWorldModel() -{ - // update map size - - if ( - map.getLength().x() != world_model->field_size.x() || - map.getLength().y() != world_model->field_size.y()) { - map.clearAll(); - map.setGeometry( - grid_map::Length(world_model->field_size.x(), world_model->field_size.y()), MAP_RESOLUTION); - } - - // DefenseSize更新時にdefense_areaを更新する - static Vector2 defense_area_size; - if ( - defense_area_size.x() != world_model->defense_area_size.x() || - defense_area_size.y() != world_model->defense_area_size.y()) { - defense_area_size = world_model->defense_area_size; - if (not map.exists("defense_area")) { - map.add("defense_area"); - } - - for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { - grid_map::Position position; - map.getPosition(*iterator, position); - map.at("defense_area", *iterator) = world_model->isDefenseArea(position); - } - } - - // ボールプレイスメントMap - if (not map.exists("ball_placement")) { - map.add("ball_placement"); - } - for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { - grid_map::Position position; - map.getPosition(*iterator, position); - map.at("ball_placement", *iterator) = world_model->isBallPlacementArea(position); - } - - // 味方ロボットMap - if (not map.exists("friend_robot")) { - map.add("friend_robot"); - } - map["friend_robot"].setConstant(0.0); - for (const auto & robot : world_model->ours.getAvailableRobots()) { - for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); - ++iterator) { - map.at("friend_robot", *iterator) = 1.0; - } - } - - // 敵ロボットMap - if (not map.exists("enemy_robot")) { - map.add("enemy_robot"); - } - map["enemy_robot"].setConstant(0.0); - for (const auto & robot : world_model->theirs.getAvailableRobots()) { - for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); - ++iterator) { - map.at("enemy_robot", *iterator) = 1.0; - } - } - - // ボールMap - if (not map.exists("ball")) { - map.add("ball"); - } - map["ball"].setConstant(0.0); - for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.1); !iterator.isPastEnd(); - ++iterator) { - map.at("ball", *iterator) = 1.0; - } - - // ボールMap (時間) - if (not map.exists("ball_time")) { - map.add("ball_time"); - } - Vector2 ball_vel_unit = world_model->ball.vel.normalized() * MAP_RESOLUTION; - Point ball_pos = world_model->ball.pos; - double time = 0.0; - const double TIME_STEP = MAP_RESOLUTION / world_model->ball.vel.norm(); - map["ball_time"].setConstant(100.0); - for (int i = 0; i < 100; ++i) { - for (grid_map::CircleIterator iterator(map, ball_pos, 0.05); !iterator.isPastEnd(); - ++iterator) { - map.at("ball_time", *iterator) = std::min(map.at("ball_time", *iterator), (float)time); - } - ball_pos += ball_vel_unit; - time += TIME_STEP; - } - map.setTimestamp(now().nanoseconds()); - std::unique_ptr message; - message = grid_map::GridMapRosConverter::toMessage(map); - static auto publisher = - create_publisher("local_planner/grid_map", 1); - publisher->publish(std::move(message)); + commands_pub->publish(calculate_control_target(msg)); } } // namespace crane From 8c42945a21afc66ec2a28a881d71587fbea46804 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 9 Dec 2023 17:37:37 +0900 Subject: [PATCH 10/15] =?UTF-8?q?=E3=83=93=E3=83=AB=E3=83=89=E9=80=9A?= =?UTF-8?q?=E3=81=A3=E3=81=9F=EF=BC=81=E5=8B=9D=E3=81=A1=E3=81=BE=E3=81=97?= =?UTF-8?q?=E3=81=9F=EF=BC=81=EF=BC=81=EF=BC=88=E5=8B=9D=E3=81=A3=E3=81=9F?= =?UTF-8?q?=E3=81=A8=E3=81=AF=E8=A8=80=E3=81=A3=E3=81=A6=E3=81=84=E3=81=AA?= =?UTF-8?q?=E3=81=84=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_local_planner/gridmap_planner.hpp | 198 +++++++++--------- .../crane_local_planner/local_planner.hpp | 40 ++-- .../crane_local_planner/rvo_planner.hpp | 49 +++-- .../crane_local_planner/simple_planner.hpp | 29 ++- 4 files changed, 166 insertions(+), 150 deletions(-) diff --git a/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp b/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp index ff1d4587b..ee6816943 100644 --- a/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp @@ -7,124 +7,130 @@ #ifndef CRANE_GRIDMAP_PLANNER_HPP #define CRANE_GRIDMAP_PLANNER_HPP -#include #include #include +#include -class GridMapPlanner { - public: - GridMapPlanner(rclcpp::Node & node){ - node.declare_parameter("map_resolution", MAP_RESOLUTION); - MAP_RESOLUTION = get_parameter("map_resolution").as_double(); - - node.gridmap_publisher = - create_publisher("local_planner/grid_map", 1); - map.setFrameId("map"); - map.setGeometry(grid_map::Length(1.2, 2.0), MAP_RESOLUTION, grid_map::Position(0.0, 0.0)); +namespace crane +{ +class GridMapPlanner +{ +public: + GridMapPlanner(rclcpp::Node & node): map({"penalty", "ball_placement", "theirs", "ours", "ball"}) + { + node.declare_parameter("map_resolution", MAP_RESOLUTION); + MAP_RESOLUTION = node.get_parameter("map_resolution").as_double(); + + gridmap_publisher = + node.create_publisher("local_planner/grid_map", 1); + map.setFrameId("map"); + map.setGeometry(grid_map::Length(1.2, 2.0), MAP_RESOLUTION, grid_map::Position(0.0, 0.0)); + } + + crane_msgs::msg::RobotCommands calculateControlTarget( + const crane_msgs::msg::RobotCommands &, WorldModelWrapper::SharedPtr world_model) + { + // update map size + + if ( + map.getLength().x() != world_model->field_size.x() || + map.getLength().y() != world_model->field_size.y()) { + map.clearAll(); + map.setGeometry( + grid_map::Length(world_model->field_size.x(), world_model->field_size.y()), MAP_RESOLUTION); } - crane_msgs::msg::RobotCommands calculateControlTarget(const crane_msgs::msg::RobotCommands &, WorldModelWrapper::SharedPtr world_model){ - // update map size - - if ( - map.getLength().x() != world_model->field_size.x() || - map.getLength().y() != world_model->field_size.y()) { - map.clearAll(); - map.setGeometry( - grid_map::Length(world_model->field_size.x(), world_model->field_size.y()), MAP_RESOLUTION); + // DefenseSize更新時にdefense_areaを更新する + static Vector2 defense_area_size; + if ( + defense_area_size.x() != world_model->defense_area_size.x() || + defense_area_size.y() != world_model->defense_area_size.y()) { + defense_area_size = world_model->defense_area_size; + if (not map.exists("defense_area")) { + map.add("defense_area"); } - // DefenseSize更新時にdefense_areaを更新する - static Vector2 defense_area_size; - if ( - defense_area_size.x() != world_model->defense_area_size.x() || - defense_area_size.y() != world_model->defense_area_size.y()) { - defense_area_size = world_model->defense_area_size; - if (not map.exists("defense_area")) { - map.add("defense_area"); - } - - for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { - grid_map::Position position; - map.getPosition(*iterator, position); - map.at("defense_area", *iterator) = world_model->isDefenseArea(position); - } - } - - // ボールプレイスメントMap - if (not map.exists("ball_placement")) { - map.add("ball_placement"); - } for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { grid_map::Position position; map.getPosition(*iterator, position); - map.at("ball_placement", *iterator) = world_model->isBallPlacementArea(position); + map.at("defense_area", *iterator) = world_model->isDefenseArea(position); } + } - // 味方ロボットMap - if (not map.exists("friend_robot")) { - map.add("friend_robot"); - } - map["friend_robot"].setConstant(0.0); - for (const auto & robot : world_model->ours.getAvailableRobots()) { - for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); - ++iterator) { - map.at("friend_robot", *iterator) = 1.0; - } - } + // ボールプレイスメントMap + if (not map.exists("ball_placement")) { + map.add("ball_placement"); + } + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + grid_map::Position position; + map.getPosition(*iterator, position); + map.at("ball_placement", *iterator) = world_model->isBallPlacementArea(position); + } - // 敵ロボットMap - if (not map.exists("enemy_robot")) { - map.add("enemy_robot"); - } - map["enemy_robot"].setConstant(0.0); - for (const auto & robot : world_model->theirs.getAvailableRobots()) { - for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); - ++iterator) { - map.at("enemy_robot", *iterator) = 1.0; - } + // 味方ロボットMap + if (not map.exists("friend_robot")) { + map.add("friend_robot"); + } + map["friend_robot"].setConstant(0.0); + for (const auto & robot : world_model->ours.getAvailableRobots()) { + for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); + ++iterator) { + map.at("friend_robot", *iterator) = 1.0; } + } - // ボールMap - if (not map.exists("ball")) { - map.add("ball"); - } - map["ball"].setConstant(0.0); - for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.1); !iterator.isPastEnd(); + // 敵ロボットMap + if (not map.exists("enemy_robot")) { + map.add("enemy_robot"); + } + map["enemy_robot"].setConstant(0.0); + for (const auto & robot : world_model->theirs.getAvailableRobots()) { + for (grid_map::CircleIterator iterator(map, robot->pose.pos, 0.1); !iterator.isPastEnd(); ++iterator) { - map.at("ball", *iterator) = 1.0; + map.at("enemy_robot", *iterator) = 1.0; } + } - // ボールMap (時間) - if (not map.exists("ball_time")) { - map.add("ball_time"); - } - Vector2 ball_vel_unit = world_model->ball.vel.normalized() * MAP_RESOLUTION; - Point ball_pos = world_model->ball.pos; - double time = 0.0; - const double TIME_STEP = MAP_RESOLUTION / world_model->ball.vel.norm(); - map["ball_time"].setConstant(100.0); - for (int i = 0; i < 100; ++i) { - for (grid_map::CircleIterator iterator(map, ball_pos, 0.05); !iterator.isPastEnd(); - ++iterator) { - map.at("ball_time", *iterator) = std::min(map.at("ball_time", *iterator), (float)time); - } - ball_pos += ball_vel_unit; - time += TIME_STEP; - } - map.setTimestamp(now().nanoseconds()); - std::unique_ptr message; - message = grid_map::GridMapRosConverter::toMessage(map); + // ボールMap + if (not map.exists("ball")) { + map.add("ball"); + } + map["ball"].setConstant(0.0); + for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.1); !iterator.isPastEnd(); + ++iterator) { + map.at("ball", *iterator) = 1.0; + } - gridmap_publisher->publish(std::move(message)); + // ボールMap (時間) + if (not map.exists("ball_time")) { + map.add("ball_time"); + } + Vector2 ball_vel_unit = world_model->ball.vel.normalized() * MAP_RESOLUTION; + Point ball_pos = world_model->ball.pos; + double time = 0.0; + const double TIME_STEP = MAP_RESOLUTION / world_model->ball.vel.norm(); + map["ball_time"].setConstant(100.0); + for (int i = 0; i < 100; ++i) { + for (grid_map::CircleIterator iterator(map, ball_pos, 0.05); !iterator.isPastEnd(); + ++iterator) { + map.at("ball_time", *iterator) = std::min(map.at("ball_time", *iterator), (float)time); + } + ball_pos += ball_vel_unit; + time += TIME_STEP; } + // map.setTimestamp(now().nanoseconds()); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(map); - private: - rclcpp::Publisher::SharedPtr gridmap_publisher; + gridmap_publisher->publish(std::move(message)); + } - grid_map::GridMap map; +private: + rclcpp::Publisher::SharedPtr gridmap_publisher; - double MAP_RESOLUTION = 0.05; + grid_map::GridMap map; -} + double MAP_RESOLUTION = 0.05; +}; +} // namespace crane #endif //CRANE_GRIDMAP_PLANNER_HPP diff --git a/crane_local_planner/include/crane_local_planner/local_planner.hpp b/crane_local_planner/include/crane_local_planner/local_planner.hpp index 78682ef1d..4ef50765b 100644 --- a/crane_local_planner/include/crane_local_planner/local_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/local_planner.hpp @@ -7,7 +7,6 @@ #ifndef CRANE_LOCAL_PLANNER__LOCAL_PLANNER_HPP_ #define CRANE_LOCAL_PLANNER__LOCAL_PLANNER_HPP_ - #include #include #include @@ -16,9 +15,9 @@ #include #include -#include "simple_planner.hpp" -#include "rvo_planner.hpp" #include "gridmap_planner.hpp" +#include "rvo_planner.hpp" +#include "simple_planner.hpp" #include "visibility_control.h" namespace crane @@ -35,33 +34,36 @@ class LocalPlannerComponent : public rclcpp::Node public: COMPOSITION_PUBLIC explicit LocalPlannerComponent(const rclcpp::NodeOptions & options) - : rclcpp::Node("local_planner", options), - map({"penalty", "ball_placement", "theirs", "ours", "ball"}) + : rclcpp::Node("local_planner", options) { declare_parameter("planner", "simple"); auto planner_str = get_parameter("enable_rvo").as_string(); - if(planner_str == "simple"){ + if (planner_str == "simple") { simple_planner = std::make_shared(*this); - calculate_control_target = [this](const crane_msgs::msg::RobotCommands & msg){ - return simple_planner->calculateControlTarget(msg,world_model); + calculate_control_target = + [this](const crane_msgs::msg::RobotCommands & msg) -> crane_msgs::msg::RobotCommands { + return simple_planner->calculateControlTarget(msg, world_model); }; - }else if(planner_str == "rvo"){ + } else if (planner_str == "rvo") { rvo_planner = std::make_shared(*this); - calculate_control_target = [this](const crane_msgs::msg::RobotCommands & msg){ - return rvo_planner->calculateControlTarget(msg,world_model); + calculate_control_target = + [this](const crane_msgs::msg::RobotCommands & msg) -> crane_msgs::msg::RobotCommands { + return rvo_planner->calculateControlTarget(msg, world_model); }; - }else if(planner_str == "gridmap"){ + } else if (planner_str == "gridmap") { gridmap_planner = std::make_shared(*this); - calculate_control_target = [this](const crane_msgs::msg::RobotCommands & msg){ - return gridmap_planner->calculateControlTarget(msg,world_model); + calculate_control_target = + [this](const crane_msgs::msg::RobotCommands & msg) -> crane_msgs::msg::RobotCommands { + return gridmap_planner->calculateControlTarget(msg, world_model); }; + } else { + RCLCPP_ERROR(get_logger(), "Unknown planner: %s", planner_str.c_str()); + throw std::runtime_error("Unknown planner: " + planner_str); } world_model = std::make_shared(*this); - world_model->addCallback([this]() { updateAvoidanceMapOnWorldModel(); }); - commands_pub = this->create_publisher("/robot_commands", 10); control_targets_sub = this->create_subscription( "/control_targets", 10, @@ -70,8 +72,6 @@ class LocalPlannerComponent : public rclcpp::Node void callbackControlTarget(const crane_msgs::msg::RobotCommands &); - void updateAvoidanceMapOnWorldModel(); - private: rclcpp::Subscription::SharedPtr control_targets_sub; @@ -79,12 +79,12 @@ class LocalPlannerComponent : public rclcpp::Node WorldModelWrapper::SharedPtr world_model; - std::function calculate_control_target; + std::function + calculate_control_target; std::shared_ptr simple_planner = nullptr; std::shared_ptr rvo_planner = nullptr; std::shared_ptr gridmap_planner = nullptr; - }; } // namespace crane diff --git a/crane_local_planner/include/crane_local_planner/rvo_planner.hpp b/crane_local_planner/include/crane_local_planner/rvo_planner.hpp index 400244631..6dbf73fcd 100644 --- a/crane_local_planner/include/crane_local_planner/rvo_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/rvo_planner.hpp @@ -7,34 +7,37 @@ #ifndef CRANE_RVO_PLANNER_HPP #define CRANE_RVO_PLANNER_HPP -#include "RVO.h" #include #include -class RVOPlanner{ +#include "RVO.h" +namespace crane +{ +class RVOPlanner +{ public: - RVOPlanner(rclcpp::Node & node){ - + RVOPlanner(rclcpp::Node & node) + { node.declare_parameter("rvo_time_step", RVO_TIME_STEP); - RVO_TIME_STEP = get_parameter("rvo_time_step").as_double(); + RVO_TIME_STEP = node.get_parameter("rvo_time_step").as_double(); node.declare_parameter("rvo_neighbor_dist", RVO_NEIGHBOR_DIST); - RVO_NEIGHBOR_DIST = get_parameter("rvo_neighbor_dist").as_double(); + RVO_NEIGHBOR_DIST = node.get_parameter("rvo_neighbor_dist").as_double(); node.declare_parameter("rvo_max_neighbors", RVO_MAX_NEIGHBORS); - RVO_MAX_NEIGHBORS = get_parameter("rvo_max_neighbors").as_int(); + RVO_MAX_NEIGHBORS = node.get_parameter("rvo_max_neighbors").as_int(); node.declare_parameter("rvo_time_horizon", RVO_TIME_HORIZON); - RVO_TIME_HORIZON = get_parameter("rvo_time_horizon").as_double(); + RVO_TIME_HORIZON = node.get_parameter("rvo_time_horizon").as_double(); node.declare_parameter("rvo_time_horizon_obst", RVO_TIME_HORIZON_OBST); - RVO_TIME_HORIZON_OBST = get_parameter("rvo_time_horizon_obst").as_double(); + RVO_TIME_HORIZON_OBST = node.get_parameter("rvo_time_horizon_obst").as_double(); node.declare_parameter("rvo_radius", RVO_RADIUS); - RVO_RADIUS = get_parameter("rvo_radius").as_double(); + RVO_RADIUS = node.get_parameter("rvo_radius").as_double(); node.declare_parameter("rvo_max_speed", RVO_MAX_SPEED); - RVO_MAX_SPEED = get_parameter("rvo_max_speed").as_double(); + RVO_MAX_SPEED = node.get_parameter("rvo_max_speed").as_double(); node.declare_parameter("rvo_trapezoidal_max_acc", RVO_TRAPEZOIDAL_MAX_ACC); - RVO_TRAPEZOIDAL_MAX_ACC = get_parameter("rvo_trapezoidal_max_acc").as_double(); + RVO_TRAPEZOIDAL_MAX_ACC = node.get_parameter("rvo_trapezoidal_max_acc").as_double(); node.declare_parameter("rvo_trapezoidal_frame_rate", RVO_TRAPEZOIDAL_FRAME_RATE); - RVO_TRAPEZOIDAL_FRAME_RATE = get_parameter("rvo_trapezoidal_frame_rate").as_double(); + RVO_TRAPEZOIDAL_FRAME_RATE = node.get_parameter("rvo_trapezoidal_frame_rate").as_double(); node.declare_parameter("rvo_trapezoidal_max_speed", RVO_TRAPEZOIDAL_MAX_SPEED); - RVO_TRAPEZOIDAL_MAX_SPEED = get_parameter("rvo_trapezoidal_max_speed").as_double(); + RVO_TRAPEZOIDAL_MAX_SPEED = node.get_parameter("rvo_trapezoidal_max_speed").as_double(); rvo_sim = std::make_unique( RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST, @@ -51,7 +54,8 @@ class RVOPlanner{ } } - void reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands &) + void reflectWorldToRVOSim( + const crane_msgs::msg::RobotCommands & msg, WorldModelWrapper::SharedPtr world_model) { bool add_ball = true; // 味方ロボット：RVO内の位置・速度（＝進みたい方向）の更新 @@ -180,7 +184,7 @@ class RVOPlanner{ } crane_msgs::msg::RobotCommands extractRobotCommandsFromRVOSim( - const crane_msgs::msg::RobotCommands & msg) + const crane_msgs::msg::RobotCommands & msg, WorldModelWrapper::SharedPtr world_model) { crane_msgs::msg::RobotCommands commands = msg; for (size_t i = 0; i < msg.robot_commands.size(); i++) { @@ -210,14 +214,13 @@ class RVOPlanner{ return commands; } - crane_msgs::msg::RobotCommands calculateControlTarget(const crane_msgs::msg::RobotCommands &, WorldModelWrapper::SharedPtr world_model){ - if (!world_model->hasUpdated()) { - return; - } - reflectWorldToRVOSim(msg); + crane_msgs::msg::RobotCommands calculateControlTarget( + const crane_msgs::msg::RobotCommands & msg, WorldModelWrapper::SharedPtr world_model) + { + reflectWorldToRVOSim(msg, world_model); // RVOシミュレータ更新 rvo_sim->doStep(); - return extractRobotCommandsFromRVOSim(msg); + return extractRobotCommandsFromRVOSim(msg, world_model); } private: @@ -235,5 +238,5 @@ class RVOPlanner{ float RVO_TRAPEZOIDAL_FRAME_RATE = 60; float RVO_TRAPEZOIDAL_MAX_SPEED = 4.0; }; - +} // namespace crane #endif //CRANE_RVO_PLANNER_HPP diff --git a/crane_local_planner/include/crane_local_planner/simple_planner.hpp b/crane_local_planner/include/crane_local_planner/simple_planner.hpp index 14527d888..b0a71d80e 100644 --- a/crane_local_planner/include/crane_local_planner/simple_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/simple_planner.hpp @@ -11,18 +11,22 @@ #include #include -class SimplePlanner{ +namespace crane +{ +class SimplePlanner +{ public: - SimplePlanner(rclcpp::Node & node){ + SimplePlanner(rclcpp::Node & node) + { node.declare_parameter("non_rvo_max_vel", NON_RVO_MAX_VEL); - NON_RVO_MAX_VEL = get_parameter("non_rvo_max_vel").as_double(); - + NON_RVO_MAX_VEL = node.get_parameter("non_rvo_max_vel").as_double(); + node.declare_parameter("non_rvo_p_gain", NON_RVO_P_GAIN); - NON_RVO_P_GAIN = get_parameter("non_rvo_p_gain").as_double(); + NON_RVO_P_GAIN = node.get_parameter("non_rvo_p_gain").as_double(); node.declare_parameter("non_rvo_i_gain", NON_RVO_I_GAIN); - NON_RVO_I_GAIN = get_parameter("non_rvo_i_gain").as_double(); + NON_RVO_I_GAIN = node.get_parameter("non_rvo_i_gain").as_double(); node.declare_parameter("non_rvo_d_gain", NON_RVO_D_GAIN); - NON_RVO_D_GAIN = get_parameter("non_rvo_d_gain").as_double(); + NON_RVO_D_GAIN = node.get_parameter("non_rvo_d_gain").as_double(); for (auto & controller : vx_controllers) { controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN); @@ -33,14 +37,16 @@ class SimplePlanner{ } } - crane_msgs::msg::RobotCommands calculateControlTarget(const crane_msgs::msg::RobotCommands & msg, WorldModelWrapper::SharedPtr world_model){ + crane_msgs::msg::RobotCommands calculateControlTarget( + const crane_msgs::msg::RobotCommands & msg, WorldModelWrapper::SharedPtr world_model) + { crane_msgs::msg::RobotCommands commands = msg; for (auto & command : commands.robot_commands) { if ((not command.target_x.empty()) && (not command.target_y.empty())) { auto robot = world_model->getOurRobot(command.robot_id); Point target; target << command.target_x.front(), command.target_y.front(); - + Point robot_to_target = target - robot->pose.pos; // double dot1 = (world_model->ball.pos - robot->pose.pos).dot(robot_to_target); @@ -105,14 +111,15 @@ class SimplePlanner{ } return commands; } + private: std::array vx_controllers; std::array vy_controllers; - + double NON_RVO_MAX_VEL = 4.0; double NON_RVO_P_GAIN = 4.0; double NON_RVO_I_GAIN = 0.0; double NON_RVO_D_GAIN = 0.0; }; - +} // namespace crane #endif //CRANE_SIMPLE_PLANNER_HPP From 39b30c340ecdcc3dab5c12b4895a03953fc7c3fe Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 9 Dec 2023 17:39:16 +0900 Subject: [PATCH 11/15] =?UTF-8?q?=E3=83=95=E3=82=A9=E2=88=92=E3=83=9E?= =?UTF-8?q?=E3=83=83=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_local_planner/gridmap_planner.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp b/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp index ee6816943..1fbff5545 100644 --- a/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp @@ -16,7 +16,7 @@ namespace crane class GridMapPlanner { public: - GridMapPlanner(rclcpp::Node & node): map({"penalty", "ball_placement", "theirs", "ours", "ball"}) + GridMapPlanner(rclcpp::Node & node) : map({"penalty", "ball_placement", "theirs", "ours", "ball"}) { node.declare_parameter("map_resolution", MAP_RESOLUTION); MAP_RESOLUTION = node.get_parameter("map_resolution").as_double(); From 95de52442b23035f9351939e7308a6fe1ef33b4c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 9 Dec 2023 17:51:33 +0900 Subject: [PATCH 12/15] =?UTF-8?q?=E3=83=93=E3=83=AB=E3=83=89=E3=83=86?= =?UTF-8?q?=E3=82=B9=E3=83=88=E3=81=AF=E3=82=BD=E3=83=BC=E3=82=B9=E3=82=B3?= =?UTF-8?q?=E3=83=BC=E3=83=89=E5=BC=84=E3=81=A3=E3=81=9F=E3=81=A8=E3=81=8D?= =?UTF-8?q?=E3=81=A0=E3=81=91=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/build_test.yaml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.github/workflows/build_test.yaml b/.github/workflows/build_test.yaml index 00c619cbc..13104b570 100644 --- a/.github/workflows/build_test.yaml +++ b/.github/workflows/build_test.yaml @@ -4,6 +4,13 @@ name: build test on: workflow_dispatch: pull_request: + paths: + - '.github/workflows/build-test.yml' + - '**/**.cpp' + - '**/**.h' + - '**/**.hpp' + - '**/CMakeLists.txt' + - '**/package.xml' merge_group: From ec6b70fa6d6a2c3a007a590fc1e15b69e6bd7a7c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 9 Dec 2023 17:53:08 +0900 Subject: [PATCH 13/15] =?UTF-8?q?=E3=83=AD=E3=83=BC=E3=82=AB=E3=83=AB?= =?UTF-8?q?=E3=83=97=E3=83=A9=E3=83=B3=E3=83=8A=E5=88=87=E3=82=8A=E6=9B=BF?= =?UTF-8?q?=E3=81=88=E3=83=91=E3=83=A9=E3=83=A1=E3=83=BC=E3=82=BF=E3=82=92?= =?UTF-8?q?launch=E3=81=AB=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/play_switcher.launch.py | 2 +- crane_bringup/launch/play_switcher_y.launch.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_bringup/launch/play_switcher.launch.py b/crane_bringup/launch/play_switcher.launch.py index 470361b45..bdbd8827c 100755 --- a/crane_bringup/launch/play_switcher.launch.py +++ b/crane_bringup/launch/play_switcher.launch.py @@ -111,7 +111,7 @@ def generate_launch_description(): on_exit=ShutdownOnce(), parameters=[ { - "enable_rvo": False, + "planner": "simple", # "non_rvo_gain": 2.15, "non_rvo_p_gain": 2.9, "non_rvo_d_gain": 1.0, diff --git a/crane_bringup/launch/play_switcher_y.launch.py b/crane_bringup/launch/play_switcher_y.launch.py index 86bc585bb..d2eb82089 100755 --- a/crane_bringup/launch/play_switcher_y.launch.py +++ b/crane_bringup/launch/play_switcher_y.launch.py @@ -110,7 +110,7 @@ def generate_launch_description(): on_exit=ShutdownOnce(), parameters=[ { - "enable_rvo": False, + "planner": "simple", "non_rvo_gain": 2.15, } ], From c65d23f7c118274f59a235c72a1dd892787e459f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 9 Dec 2023 18:03:42 +0900 Subject: [PATCH 14/15] =?UTF-8?q?=E7=BD=AA=E6=B7=B1=E3=81=84=E3=83=9F?= =?UTF-8?q?=E3=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_local_planner/local_planner.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_local_planner/include/crane_local_planner/local_planner.hpp b/crane_local_planner/include/crane_local_planner/local_planner.hpp index 4ef50765b..1202903b9 100644 --- a/crane_local_planner/include/crane_local_planner/local_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/local_planner.hpp @@ -37,7 +37,7 @@ class LocalPlannerComponent : public rclcpp::Node : rclcpp::Node("local_planner", options) { declare_parameter("planner", "simple"); - auto planner_str = get_parameter("enable_rvo").as_string(); + auto planner_str = get_parameter("planner").as_string(); if (planner_str == "simple") { simple_planner = std::make_shared(*this); From c3c5a5817d707248b854a704167343de68e5be4a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 16 Dec 2023 17:35:33 +0900 Subject: [PATCH 15/15] =?UTF-8?q?OBST=E3=82=92=E7=84=A1=E8=A6=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_local_planner/include/crane_local_planner/rvo_planner.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/crane_local_planner/include/crane_local_planner/rvo_planner.hpp b/crane_local_planner/include/crane_local_planner/rvo_planner.hpp index 6dbf73fcd..d8421e21f 100644 --- a/crane_local_planner/include/crane_local_planner/rvo_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/rvo_planner.hpp @@ -11,6 +11,8 @@ #include #include "RVO.h" + +// cspell: ignore OBST namespace crane { class RVOPlanner