diff --git a/CMakeLists.txt b/CMakeLists.txt index 03e38d7..b8cace3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,8 @@ find_package(jitsuyo REQUIRED) find_package(kansei REQUIRED) find_package(kansei_interfaces REQUIRED) find_package(keisan REQUIRED) +find_package(ninshiki_cpp REQUIRED) +find_package(ninshiki_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(basho_interfaces REQUIRED) @@ -48,6 +50,8 @@ ament_target_dependencies(${PROJECT_NAME} kansei kansei_interfaces keisan + ninshiki_cpp + ninshiki_interfaces rclcpp std_msgs basho_interfaces) @@ -85,6 +89,8 @@ ament_export_dependencies( kansei kansei_interfaces keisan + ninshiki_cpp + ninshiki_interfaces rclcpp std_msgs basho_interfaces) diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp index 28285fa..f197c78 100644 --- a/include/basho/localization/mcl.hpp +++ b/include/basho/localization/mcl.hpp @@ -33,8 +33,14 @@ namespace basho struct ProjectedObject { + float score; + float left; + float top; + float width; + float height; std::string label; - keisan::Point3 position; + keisan::Point2 position; + bool has_projection; }; struct Particle @@ -85,6 +91,7 @@ class MCL void set_odometry_position(const keisan::Point2 & odom) { odometry_position = odom; } + // MCL configuration int num_particles; double min_centered_ratio; double short_term_avg_ratio; @@ -93,8 +100,24 @@ class MCL double range_sigma; double chi_square_gate; double uncertainty_threshold; + + // Gating configuration keisan::Point2 max_object_distance; + double max_roll_angle; + double max_pitch_angle; + double max_pan_angle; + double max_tilt_angle; + double confidence_threshold; + int min_num_observations; + + // Landmark configuration bool generalized_landmark; + bool enable_x_intersection; + bool enable_l_intersection; + bool enable_t_intersection; + bool enable_goalpost; + + // Debug bool print_debug; bool apply_localization; diff --git a/include/basho/model/field.hpp b/include/basho/model/field.hpp index ec8a834..f3b5225 100644 --- a/include/basho/model/field.hpp +++ b/include/basho/model/field.hpp @@ -37,7 +37,6 @@ struct Field std::vector landmarks_T; std::vector landmarks_X; std::vector landmarks_goalpost; - std::vector landmarks_intersection; Field() : width(600), @@ -82,10 +81,25 @@ struct Field {900.0, 390.0} // Right goal - lower goalpost (on goal line) }) { - landmarks_intersection.reserve(landmarks_L.size() + landmarks_T.size() + landmarks_X.size()); - landmarks_intersection.insert(landmarks_intersection.end(), landmarks_L.begin(), landmarks_L.end()); - landmarks_intersection.insert(landmarks_intersection.end(), landmarks_T.begin(), landmarks_T.end()); - landmarks_intersection.insert(landmarks_intersection.end(), landmarks_X.begin(), landmarks_X.end()); + } + + std::vector get_intersections(bool use_l, bool use_t, bool use_x) + { + std::vector intersections; + + size_t reserve_size = 0; + + if (use_l) reserve_size += landmarks_L.size(); + if (use_t) reserve_size += landmarks_T.size(); + if (use_x) reserve_size += landmarks_X.size(); + + intersections.reserve(reserve_size); + + if (use_l) intersections.insert(intersections.end(), landmarks_L.begin(), landmarks_L.end()); + if (use_t) intersections.insert(intersections.end(), landmarks_T.begin(), landmarks_T.end()); + if (use_x) intersections.insert(intersections.end(), landmarks_X.begin(), landmarks_X.end()); + + return intersections; } }; diff --git a/include/basho/node/basho_node.hpp b/include/basho/node/basho_node.hpp index 848a8c9..1aef555 100644 --- a/include/basho/node/basho_node.hpp +++ b/include/basho/node/basho_node.hpp @@ -35,6 +35,7 @@ #include "basho_interfaces/msg/particles.hpp" #include "gyakuenki_interfaces/msg/projected_objects.hpp" #include "kansei_interfaces/msg/status.hpp" +#include "ninshiki_interfaces/msg/contours.hpp" namespace basho { @@ -49,6 +50,7 @@ class BashoNode using Point2 = aruku_interfaces::msg::Point2; using WalkingStatus = aruku_interfaces::msg::Status; using HeadData = atama_interfaces::msg::Head; + using Contours = ninshiki_interfaces::msg::Contours; static std::string get_node_prefix(); @@ -78,6 +80,7 @@ class BashoNode rclcpp::Subscription::SharedPtr delta_position_subscriber; rclcpp::Subscription::SharedPtr init_localization_subscriber; rclcpp::Subscription::SharedPtr head_data_subscriber; + rclcpp::Subscription::SharedPtr contours_subscriber; std::shared_ptr localization; diff --git a/include/basho/process/localization.hpp b/include/basho/process/localization.hpp index 9759a52..6363a7a 100644 --- a/include/basho/process/localization.hpp +++ b/include/basho/process/localization.hpp @@ -28,6 +28,7 @@ #include "nlohmann/json.hpp" #include "basho/localization/mcl.hpp" #include "basho/localization/fusion.hpp" +#include "ninshiki_cpp/utils/contours.hpp" namespace basho { @@ -49,12 +50,17 @@ class Localization std::shared_ptr get_mcl() const; + void set_field_contours(const ninshiki_cpp::utils::Contours & contours); + bool is_landmark_valid(const ProjectedObject & obj); + std::string config_path; std::string config_name; private: std::shared_ptr mcl; Fusion2D fusion; + + ninshiki_cpp::utils::Contours field_contours; }; } // namespace basho diff --git a/package.xml b/package.xml index 27e2e3d..27bc0e4 100644 --- a/package.xml +++ b/package.xml @@ -14,6 +14,8 @@ kansei kansei_interfaces keisan + ninshiki_cpp + ninshiki_interfaces nlohmann-json-dev rclcpp std_msgs diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp index 4461baa..26c0309 100644 --- a/src/basho/localization/mcl.cpp +++ b/src/basho/localization/mcl.cpp @@ -54,10 +54,11 @@ void MCL::localize() update_motion(); - bool evaluate_particles = num_projected_objects > 0; - evaluate_particles &= fabs(orientation_roll.degree()) <= 30; - evaluate_particles &= fabs(orientation_pitch.degree()) <= 30; - evaluate_particles &= fabs(head_pan.degree()) <= 60; + bool evaluate_particles = num_projected_objects >= min_num_observations; + evaluate_particles &= fabs(orientation_roll.degree()) <= max_roll_angle; + evaluate_particles &= fabs(orientation_pitch.degree()) <= max_pitch_angle; + evaluate_particles &= fabs(head_pan.degree()) <= max_pan_angle; + evaluate_particles &= fabs(head_tilt.degree()) <= max_tilt_angle; apply_localization = false; @@ -134,7 +135,6 @@ void MCL::clear_projected_objects() projected_X.clear(); projected_L.clear(); projected_T.clear(); - projected_intersection.clear(); projected_goalpost.clear(); num_projected_objects = 0; } @@ -144,7 +144,7 @@ void MCL::update_motion() double k_trans = 0.05; double min_trans_noise = 0.5; - double sigma_theta = 5.0; + double sigma_theta = 10.0; double dx = delta_position.x; double dy = delta_position.y; @@ -217,7 +217,7 @@ void MCL::resample_particles_wheel() std::normal_distribution noise_x(0.0, resample_sigma_x); std::normal_distribution noise_y(0.0, resample_sigma_y); - std::normal_distribution noise_theta(0.0, 5.0); + std::normal_distribution noise_theta(0.0, 10.0); for (int i = 0; i < num_particles; ++i) { if (rand_prob(rand_gen) < prob) { @@ -283,7 +283,7 @@ void MCL::resample_particles_low_variance() // TOOD: get noise value from config std::normal_distribution noise_x(0.0, 75.0); std::normal_distribution noise_y(0.0, 75.0); - std::normal_distribution noise_theta(0.0, 8.0); + std::normal_distribution noise_theta(0.0, 10.0); for (int i = 0; i < num_random; ++i) { int idx = num_particles - 1 - i; @@ -297,7 +297,7 @@ void MCL::resample_particles_low_variance() (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); // Assign small weight so it does not dominate immediately - new_particles[idx].weight = 1e-3; + new_particles[idx].weight = 1e-6; } int num_inject_odom = 0; @@ -313,7 +313,7 @@ void MCL::resample_particles_low_variance() new_particles[idx].position.y = odometry_position.y + noise_y(rand_gen); new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); - new_particles[idx].weight = 1e-3; + new_particles[idx].weight = 1e-6; } } @@ -330,7 +330,7 @@ void MCL::resample_particles_low_variance() new_particles[idx].position.y = goalpost_hypothesis.y + noise_y(rand_gen); new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); - new_particles[idx].weight = 1e-3; + new_particles[idx].weight = 1e-6; } } } @@ -483,7 +483,9 @@ double MCL::calculate_total_likelihood(const Particle & particle) }; if (generalized_landmark) { - landmark_groups.push_back({&projected_intersection, &field.landmarks_intersection}); + auto field_intersections = field.get_intersections( + enable_l_intersection, enable_t_intersection, enable_x_intersection); + landmark_groups.push_back({&projected_intersection, &field_intersections}); } else { landmark_groups.push_back({&projected_X, &field.landmarks_X}); landmark_groups.push_back({&projected_L, &field.landmarks_L}); @@ -502,7 +504,7 @@ double MCL::calculate_total_likelihood(const Particle & particle) for (int j = 0; j < group.landmarks->size(); ++j) { if (result[i][j] == 1) { if (cost_matrix[i][j] >= 1e8) { - log_total += std::log(1e-3); + log_total += std::log(1e-6); valid_associations++; continue; } @@ -683,7 +685,7 @@ void MCL::print_particles() { return; } - printf("\033c"); + printf("\033[2J\033[H"); printf("Particles num: %d\n", particles.size()); printf("Prob: %.5f\n", prob); printf("Short term avg: %.8f\n", short_term_avg); diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp index 54da123..c781a69 100644 --- a/src/basho/node/basho_node.cpp +++ b/src/basho/node/basho_node.cpp @@ -77,26 +77,21 @@ BashoNode::BashoNode( 10, [this](const ProjectedObjects::SharedPtr msg) { auto mcl = this->localization->get_mcl(); - mcl->clear_projected_objects(); for (const auto & obj : msg->projected_objects) { - const bool ignore = - obj.label == "ball" || - obj.label == "robot" || - obj.label == "self" || - std::abs(obj.position.x * 100) > mcl->max_object_distance.x || - std::abs(obj.position.y * 100) > mcl->max_object_distance.y; - - if (ignore) { - continue; - } - ProjectedObject projected{ + obj.confidence, + obj.left, obj.top, obj.right, obj.bottom, obj.label, - {obj.position.x, obj.position.y, obj.position.z} + {obj.position.x, obj.position.y}, + obj.has_projection }; + if (!this->localization->is_landmark_valid(projected)) { + continue; + } + if (obj.label == "goalpost") { mcl->projected_goalpost.push_back(projected); } else if (mcl->generalized_landmark) { @@ -113,6 +108,26 @@ BashoNode::BashoNode( } }); + contours_subscriber = + node->create_subscription( + "/ninshiki_cpp/color_detection", + 10, + [this](const Contours::SharedPtr msg) { + ninshiki_cpp::utils::Contours contours; + + for (const auto & contour : msg->contours) { + if (contour.name != "field") continue; + + std::vector temp_contour; + for (const auto & point : contour.contour) { + temp_contour.emplace_back(point.x, point.y); + } + + contours.contours.push_back(temp_contour); + } + this->localization->set_field_contours(contours); + }); + init_localization_subscriber = node->create_subscription( get_node_prefix() + "/init_localization", diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp index d34b94d..2fd8595 100644 --- a/src/basho/process/localization.cpp +++ b/src/basho/process/localization.cpp @@ -55,36 +55,69 @@ void Localization::load_config() void Localization::set_config(const nlohmann::json & json) { - bool valid_config = true; - nlohmann::json localization_section; - if (jitsuyo::assign_val(json, "localization", localization_section)) { + nlohmann::json mcl_section; + if (jitsuyo::assign_val(json, "mcl", mcl_section)) { bool valid_section = true; - double max_distance_x = 0.0; - double max_distance_y = 0.0; + valid_section &= jitsuyo::assign_val(mcl_section, "num_particles", mcl->num_particles); + valid_section &= jitsuyo::assign_val(mcl_section, "min_centered_ratio", mcl->min_centered_ratio); + valid_section &= jitsuyo::assign_val(mcl_section, "short_term_avg_ratio", mcl->short_term_avg_ratio); + valid_section &= jitsuyo::assign_val(mcl_section, "long_term_avg_ratio", mcl->long_term_avg_ratio); + valid_section &= jitsuyo::assign_val(mcl_section, "base_sigma", mcl->base_sigma); + valid_section &= jitsuyo::assign_val(mcl_section, "range_sigma", mcl->range_sigma); + valid_section &= jitsuyo::assign_val(mcl_section, "uncertainty_threshold", mcl->uncertainty_threshold); + valid_section &= jitsuyo::assign_val(mcl_section, "debug", mcl->print_debug); + + if (!valid_section) { + std::cout << "Error found at section `mcl`" << std::endl; + valid_config = false; + } + } else { + std::cout << "Missing section `mcl`" << std::endl; + valid_config = false; + } + + nlohmann::json gating_section; + if (jitsuyo::assign_val(json, "gating", gating_section)) { + bool valid_section = true; + + valid_section &= jitsuyo::assign_val(gating_section, "chi_square_gate", mcl->chi_square_gate); + valid_section &= jitsuyo::assign_val(gating_section, "confidence_threshold", mcl->confidence_threshold); + valid_section &= jitsuyo::assign_val(gating_section, "max_distance_x", mcl->max_object_distance.x); + valid_section &= jitsuyo::assign_val(gating_section, "max_distance_y", mcl->max_object_distance.y); + valid_section &= jitsuyo::assign_val(gating_section, "max_roll_angle", mcl->max_roll_angle); + valid_section &= jitsuyo::assign_val(gating_section, "max_pitch_angle", mcl->max_pitch_angle); + valid_section &= jitsuyo::assign_val(gating_section, "max_pan_angle", mcl->max_pan_angle); + valid_section &= jitsuyo::assign_val(gating_section, "max_tilt_angle", mcl->max_tilt_angle); + valid_section &= jitsuyo::assign_val(gating_section, "min_num_observations", mcl->min_num_observations); + + if (!valid_section) { + std::cout << "Error found at section `gating`" << std::endl; + valid_config = false; + } + } else { + std::cout << "Missing section `gating`" << std::endl; + valid_config = false; + } - valid_section &= jitsuyo::assign_val(localization_section, "num_particles", mcl->num_particles); - valid_section &= jitsuyo::assign_val(localization_section, "min_centered_ratio", mcl->min_centered_ratio); - valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", mcl->short_term_avg_ratio); - valid_section &= jitsuyo::assign_val(localization_section, "long_term_avg_ratio", mcl->long_term_avg_ratio); - valid_section &= jitsuyo::assign_val(localization_section, "base_sigma", mcl->base_sigma); - valid_section &= jitsuyo::assign_val(localization_section, "range_sigma", mcl->range_sigma); - valid_section &= jitsuyo::assign_val(localization_section, "chi_square_gate", mcl->chi_square_gate); - valid_section &= jitsuyo::assign_val(localization_section, "max_distance_x", max_distance_x); - valid_section &= jitsuyo::assign_val(localization_section, "max_distance_y", max_distance_y); - valid_section &= jitsuyo::assign_val(localization_section, "uncertainty_threshold", mcl->uncertainty_threshold); - valid_section &= jitsuyo::assign_val(localization_section, "generalized_landmark", mcl->generalized_landmark); - valid_section &= jitsuyo::assign_val(localization_section, "debug", mcl->print_debug); + nlohmann::json landmarks_section; + if (jitsuyo::assign_val(json, "landmarks", landmarks_section)) { + bool valid_section = true; - mcl->max_object_distance = {max_distance_x, max_distance_y}; + valid_section &= jitsuyo::assign_val(landmarks_section, "generalized_landmark", mcl->generalized_landmark); + valid_section &= jitsuyo::assign_val(landmarks_section, "x_intersection", mcl->enable_x_intersection); + valid_section &= jitsuyo::assign_val(landmarks_section, "l_intersection", mcl->enable_l_intersection); + valid_section &= jitsuyo::assign_val(landmarks_section, "t_intersection", mcl->enable_t_intersection); + valid_section &= jitsuyo::assign_val(landmarks_section, "goalpost", mcl->enable_goalpost); if (!valid_section) { - std::cout << "Error found at section `localization`" << std::endl; + std::cout << "Error found at section `landmarks`" << std::endl; valid_config = false; } } else { + std::cout << "Missing section `landmarks`" << std::endl; valid_config = false; } @@ -97,19 +130,35 @@ void Localization::save_config() { nlohmann::json config; - config["localization"] = { + config["mcl"] = { {"num_particles", mcl->num_particles}, {"min_centered_ratio", mcl->min_centered_ratio}, {"short_term_avg_ratio", mcl->short_term_avg_ratio}, {"long_term_avg_ratio", mcl->long_term_avg_ratio}, {"base_sigma", mcl->base_sigma}, {"range_sigma", mcl->range_sigma}, + {"uncertainty_threshold", mcl->uncertainty_threshold}, + {"debug", mcl->print_debug} + }; + + config["gating"] = { {"chi_square_gate", mcl->chi_square_gate}, + {"confidence_threshold", mcl->confidence_threshold}, {"max_distance_x", mcl->max_object_distance.x}, {"max_distance_y", mcl->max_object_distance.y}, - {"uncertainty_threshold", mcl->uncertainty_threshold}, + {"max_roll_angle", mcl->max_roll_angle}, + {"max_pitch_angle", mcl->max_pitch_angle}, + {"min_num_observations", mcl->min_num_observations}, + {"max_pan_angle", mcl->max_pan_angle}, + {"max_tilt_angle", mcl->max_tilt_angle} + }; + + config["landmarks"] = { {"generalized_landmark", mcl->generalized_landmark}, - {"debug", mcl->print_debug} + {"x_intersection", mcl->enable_x_intersection}, + {"l_intersection", mcl->enable_l_intersection}, + {"t_intersection", mcl->enable_t_intersection}, + {"goalpost", mcl->enable_goalpost} }; jitsuyo::save_config(config_path, config_name, config); @@ -158,4 +207,71 @@ std::shared_ptr Localization::get_mcl() const return mcl; } +void Localization::set_field_contours(const ninshiki_cpp::utils::Contours & contours) +{ + field_contours = contours; + field_contours.filter_larger_than(200.0); + field_contours.join_all(); + field_contours.convex_hull(); + field_contours.strecth_up(8.0); +} + +bool Localization::is_landmark_valid(const ProjectedObject & obj) +{ + bool is_ignored_object = + obj.label == "ball" || + obj.label == "robot" || + obj.label == "self"; + + if (is_ignored_object) { + return false; + } + + if (!obj.has_projection) { + return false; + } + + if (obj.score < mcl->confidence_threshold) { + return false; + } + + bool is_disabled_landmark = + (obj.label == "X-Intersection" && !mcl->enable_x_intersection) || + (obj.label == "L-Intersection" && !mcl->enable_l_intersection) || + (obj.label == "T-Intersection" && !mcl->enable_t_intersection) || + (obj.label == "goalpost" && !mcl->enable_goalpost); + + if (is_disabled_landmark) { + return false; + } + + bool is_out_of_range = + std::abs(obj.position.x * 100) > mcl->max_object_distance.x || + std::abs(obj.position.y * 100) > mcl->max_object_distance.y; + + if (is_out_of_range) { + return false; + } + + // Check if landmark is on the field + cv::Size mat_size(320, 240); + cv::Mat field_binary_mat = field_contours.get_binary_mat(mat_size); + + if (obj.label == "goalpost") { + int y = obj.top + obj.height; + int left_x = obj.left; + int right_x = obj.left + obj.width; + + bool left_on_field = field_binary_mat.at(y, left_x) == 255; + bool right_on_field = field_binary_mat.at(y, right_x) == 255; + + return left_on_field || right_on_field; + } + + int center_x = obj.left + obj.width / 2; + int center_y = obj.top + obj.height / 2; + + return field_binary_mat.at(center_y, center_x) == 255; +} + } // namespace basho