From 7001d9116624e369df0d2634c15a8423fa7eaadb Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Tue, 19 Nov 2024 00:23:57 +0700 Subject: [PATCH 01/45] feat: add gyakuenki subscriber --- CMakeLists.txt | 3 +++ include/suiryoku/locomotion/model/robot.hpp | 11 +++++++++++ .../suiryoku/locomotion/node/locomotion_node.hpp | 4 ++++ src/suiryoku/locomotion/node/locomotion_node.cpp | 15 +++++++++++++++ 4 files changed, 33 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index f5d00c4..e685cf8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(ament_index_cpp REQUIRED) find_package(aruku REQUIRED) find_package(aruku_interfaces REQUIRED) find_package(atama_interfaces REQUIRED) +find_package(gyakuenki_interfaces REQUIRED) find_package(jitsuyo REQUIRED) find_package(kansei REQUIRED) find_package(kansei_interfaces REQUIRED) @@ -47,6 +48,7 @@ ament_target_dependencies(${PROJECT_NAME} aruku aruku_interfaces atama_interfaces + gyakuenki_interfaces jitsuyo kansei kansei_interfaces @@ -112,6 +114,7 @@ ament_export_dependencies( aruku aruku_interfaces atama_interfaces + gyakuenki_interfaces jitsuyo kansei kansei_interfaces diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 86c4517..531756d 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -28,6 +28,14 @@ namespace suiryoku { +struct ProjectedObject +{ + std::string label; + double x; + double y; + double z; +}; + class Robot { public: @@ -57,6 +65,9 @@ class Robot double y_speed; double a_speed; bool aim_on; + + // IPM + std::vector projected_objects; }; } // namespace suiryoku diff --git a/include/suiryoku/locomotion/node/locomotion_node.hpp b/include/suiryoku/locomotion/node/locomotion_node.hpp index fb2e800..d23a28a 100644 --- a/include/suiryoku/locomotion/node/locomotion_node.hpp +++ b/include/suiryoku/locomotion/node/locomotion_node.hpp @@ -28,6 +28,7 @@ #include "aruku_interfaces/msg/set_walking.hpp" #include "aruku_interfaces/msg/status.hpp" #include "atama_interfaces/msg/head.hpp" +#include "gyakuenki_interfaces/msg/projected_objects.hpp" #include "kansei_interfaces/msg/status.hpp" #include "rclcpp/rclcpp.hpp" #include "suiryoku/locomotion/model/robot.hpp" @@ -44,6 +45,7 @@ class LocomotionNode using Point2 = aruku_interfaces::msg::Point2; using SetWalking = aruku_interfaces::msg::SetWalking; using WalkingStatus = aruku_interfaces::msg::Status; + using ProjectedObjects = gyakuenki_interfaces::msg::ProjectedObjects; static std::string get_node_prefix(); @@ -69,6 +71,8 @@ class LocomotionNode rclcpp::Subscription::SharedPtr head_subscriber; + rclcpp::Subscription::SharedPtr projected_objects_subscriber; + std::shared_ptr locomotion; std::shared_ptr robot; diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index ad21df9..a38cc52 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -74,6 +74,21 @@ LocomotionNode::LocomotionNode( this->robot->tilt = keisan::make_degree(message->tilt_angle); }); + projected_objects_subscriber = node->create_subscription( + "/gyakuenki/projected_objects", 10, + [this](const ProjectedObjects::SharedPtr message) { + this->robot->projected_objects.clear(); + for (const auto & obj : message->projected_objects) { + this->robot->projected_objects.push_back( + ProjectedObject{ + obj.label, + obj.center.x, + obj.center.y, + obj.center.z + }); + } + }); + locomotion->stop = [this]() {this->walking_state = false;}; locomotion->start = [this]() {this->walking_state = true;}; } From 968b1cd470ecea8b1de34374669bd55d0610ab75 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Fri, 22 Nov 2024 03:48:54 +0700 Subject: [PATCH 02/45] feat: add localization methods and attributes --- CMakeLists.txt | 1 + include/suiryoku/locomotion/model/field.hpp | 24 +++ include/suiryoku/locomotion/model/robot.hpp | 35 +++- src/suiryoku/locomotion/model/field.cpp | 18 ++ src/suiryoku/locomotion/model/robot.cpp | 176 +++++++++++++++++- .../locomotion/node/locomotion_node.cpp | 4 +- 6 files changed, 249 insertions(+), 9 deletions(-) create mode 100644 include/suiryoku/locomotion/model/field.hpp create mode 100644 src/suiryoku/locomotion/model/field.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e685cf8..9055286 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,6 +35,7 @@ add_library(${PROJECT_NAME} SHARED "src/${PROJECT_NAME}/config/node/config_node.cpp" "src/${PROJECT_NAME}/locomotion/control/helper/parameter.cpp" "src/${PROJECT_NAME}/locomotion/control/node/control_node.cpp" + "src/${PROJECT_NAME}/locomotion/model/field.cpp" "src/${PROJECT_NAME}/locomotion/model/robot.cpp" "src/${PROJECT_NAME}/locomotion/node/locomotion_node.cpp" "src/${PROJECT_NAME}/locomotion/process/locomotion.cpp" diff --git a/include/suiryoku/locomotion/model/field.hpp b/include/suiryoku/locomotion/model/field.hpp new file mode 100644 index 0000000..70be43e --- /dev/null +++ b/include/suiryoku/locomotion/model/field.hpp @@ -0,0 +1,24 @@ +#ifndef SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_ +#define SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_ + +#include + +#include "keisan/keisan.hpp" + +namespace suiryoku +{ + +class Field +{ +public: + Field(); + + const double width; + const double length; + const double num_landmarks; + std::vector landmarks; +}; + +} // namespace suiryoku + +#endif // SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_ \ No newline at end of file diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 531756d..61475d4 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -24,6 +24,7 @@ #include #include "keisan/keisan.hpp" +#include "suiryoku/locomotion/model/field.hpp" namespace suiryoku { @@ -31,9 +32,14 @@ namespace suiryoku struct ProjectedObject { std::string label; - double x; - double y; - double z; + keisan::Point3 center; +}; + +struct Particle +{ + keisan::Point2 position; + keisan::Angle orientation; + double weight; }; class Robot @@ -44,10 +50,31 @@ class Robot keisan::Angle get_pan() const; keisan::Angle get_tilt() const; + // localizations + void localize(); + void init_particles(); + void resample_particles(); + void update_particles(); + void calculate_weight(); + void estimate_position(); + double calculate_total_likelihood(const Particle & particle); + double calculate_object_likelihood(const ProjectedObject & measurement, const Particle & particle); + double get_sum_weight(); + + Field field; + std::vector particles; + int num_particles; + bool kidnapped; + keisan::Point2 estimated_position; + + // IPM + std::vector projected_objects; + // member for getting bool is_calibrated; keisan::Angle orientation; keisan::Point2 position; + keisan::Point2 prev_position; bool is_walking; @@ -66,8 +93,6 @@ class Robot double a_speed; bool aim_on; - // IPM - std::vector projected_objects; }; } // namespace suiryoku diff --git a/src/suiryoku/locomotion/model/field.cpp b/src/suiryoku/locomotion/model/field.cpp new file mode 100644 index 0000000..efd8631 --- /dev/null +++ b/src/suiryoku/locomotion/model/field.cpp @@ -0,0 +1,18 @@ +#include "suiryoku/locomotion/model/field.hpp" + +namespace suiryoku +{ + +Field::Field() +: width(600), length(900), num_landmarks(23), landmarks({ + {0.0, 0.0}, {0.0, 80.0}, {0.0, -80.0}, {0.0, 300.0}, + {0.0, -300.0}, {240.0, 0.0}, {350.0, 250.0}, {350.0, -250.0}, + {450.0, 250.0}, {450.0, -250.0}, {450.0, 300.0}, {450.0, -300.0}, + {450.0, 130.0}, {450.0, -130.0}, {-240.0, 0.0}, {-350.0, 250.0}, + {-350.0, -250.0}, {-450.0, 250.0}, {-450.0, -250.0}, {-450.0, 300.0}, + {-450.0, -300.0}, {-450.0, 130.0}, {-450.0, -130.0}, + }) +{ +} + +} // namespace suiryoku \ No newline at end of file diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 6e18c78..0d93b7f 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -18,6 +18,8 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. +#include +#include #include #include "suiryoku/locomotion/model/robot.hpp" @@ -33,7 +35,7 @@ Robot::Robot() : pan(0_deg), tilt(0_deg), pan_center(0_deg), tilt_center(0_deg), x_speed(0.0), y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), - a_amplitude(0.0), is_calibrated(false) + a_amplitude(0.0), is_calibrated(false), kidnapped(false), num_particles(0) { } @@ -47,4 +49,176 @@ keisan::Angle Robot::get_tilt() const return tilt + tilt_center; } +void Robot::localize() +{ + if (kidnapped) { + init_particles(); + kidnapped = false; + } else { + update_particles(); + } + + calculate_weight(); + resample_particles(); + estimate_position(); +} + +void Robot::init_particles() +{ + particles.clear(); + if (!kidnapped) { + const double var_x = 10.0, var_y = 10.0, var_w = 0.05; + num_particles = 1000; + std::random_device xrd, yrd, wrd; + std::normal_distribution xrg(position.x, var_x), + yrg(position.y, var_y), wrg(orientation.degree(), var_w); + + for (int i = 0; i < num_particles; ++i) { + Particle new_particle; + new_particle.position = keisan::Point2(xrg(xrd), yrg(yrd)); + new_particle.orientation = keisan::make_degree(wrg(wrd)).normalize(); + new_particle.weight = 1.0 / num_particles; + + particles.push_back(new_particle); + } + } else { // if kidnapped, generate particles all over the field + const int x_gap = 5, y_gap = 5; + num_particles = field.width * field.length / (x_gap * y_gap); + + for (int i = -field.width / 2; i < field.width / 2; i += x_gap) { + for (int j = -field.length / 2; j < field.length / 2; j += y_gap) { + Particle new_particle; + new_particle.position = keisan::Point2(i, j); + new_particle.orientation = orientation; + new_particle.weight = 1.0 / num_particles; + + particles.push_back(new_particle); + } + } + } +} + +void Robot::resample_particles() +{ + std::vector new_particles; + std::random_device xrd, yrd, wrd; + const double var_x = 5.0, var_y = 5.0, var_w = 0.01; + + for (auto & particle : particles) { + if (particle.weight >= 1.0 / (particles.size() * 10.0)) { + new_particles.push_back(particle); + std::normal_distribution xrg(particle.position.x, var_x), + yrg(particle.position.y, var_y), wrg(particle.orientation.degree(), var_w); + + int n = particle.weight * 100; + + for (int i = 0; i < n; ++i) { + Particle new_particle; + new_particle.position = keisan::Point2(xrg(xrd), yrg(yrd)); + new_particle.orientation = keisan::make_degree(wrg(wrd)).normalize(); + new_particle.weight = 1.0 / n; + + new_particles.push_back(new_particle); + } + } + } + particles = new_particles; + num_particles = particles.size(); +} + +void Robot::update_particles() +{ + double dx = position.x - prev_position.x; + double dy = position.y - prev_position.y; + + for (int i = 0; i < num_particles; ++i) { + particles[i].position.x += dx; + particles[i].position.y += dy; + particles[i].orientation = orientation; + } + + prev_position = position; +} + +double Robot::get_sum_weight() +{ + double sum_weight = 0.0; + for (int i = 0; i < num_particles; ++i) { + sum_weight += particles[i].weight; + } + return sum_weight; +} + +void Robot::calculate_weight() +{ + for (int i = 0; i < num_particles; ++i) { + double likelihood = calculate_total_likelihood(particles[i]); + particles[i].weight = likelihood; + } + + double sum_weight = get_sum_weight(); + if (sum_weight > 0.0) { + for (int i = 0; i < num_particles; ++i) { + particles[i].weight /= sum_weight; + } + } else { + for (int i = 0; i < num_particles; ++i) { + particles[i].weight = 1 / num_particles; + } + } +} + +double Robot::calculate_total_likelihood(const Particle & particle) { + double total_likelihood = 1.0; + for (const auto & object_measurement : projected_objects) { + total_likelihood *= + calculate_object_likelihood(object_measurement, particle); + } + + return total_likelihood; +} + +double Robot::calculate_object_likelihood( + const ProjectedObject & measurement, const Particle & particle) { + const double sigma_x = 1.0, sigma_y = 1.0; + double relative_position_x, relative_position_y; + double dx, dy, x_rot, y_rot, exponent, likelihood; + double current_likelihood = 0.0; + + for (int i = 0; i < field.num_landmarks; i++) { + dx = measurement.center.x * 100; + dy = measurement.center.y * 100; + + x_rot = dx * cos(particle.orientation.degree()) - dy * sin(particle.orientation.degree()); + y_rot = dx * sin(particle.orientation.degree()) + dy * cos(particle.orientation.degree()); + + relative_position_x = particle.position.x + x_rot; + relative_position_y = particle.position.y + y_rot; + + exponent = + -0.5 * + (pow((field.landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + + pow((field.landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); + + likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y); + + if (likelihood > current_likelihood) { + current_likelihood = likelihood; + } + } + + return current_likelihood; +} + +void Robot::estimate_position() { + double x_mean = 0.0; + double y_mean = 0.0; + for (auto p : particles) { + x_mean += (1.0 / num_particles) * p.position.x; + y_mean += (1.0 / num_particles) * p.position.y; + } + estimated_position.x = x_mean; + estimated_position.y = y_mean; +} + } // namespace suiryoku diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index a38cc52..be0787b 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -82,9 +82,7 @@ LocomotionNode::LocomotionNode( this->robot->projected_objects.push_back( ProjectedObject{ obj.label, - obj.center.x, - obj.center.y, - obj.center.z + keisan::Point3{obj.center.x, obj.center.y, obj.center.z} }); } }); From 6bb8a551c1d8cbe4e643f4e1950716d7696daa1f Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Fri, 22 Nov 2024 03:52:45 +0700 Subject: [PATCH 03/45] feat: run localize when odometry updated and projected objetcs not empty --- src/suiryoku/locomotion/node/locomotion_node.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index be0787b..68080c9 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -65,6 +65,10 @@ LocomotionNode::LocomotionNode( this->robot->a_amplitude = message->a_amplitude; this->robot->position.x = message->odometry.x; this->robot->position.y = message->odometry.y; + + if (!this->robot->projected_objects.empty()) { + this->robot->localize(); + } }); head_subscriber = node->create_subscription( From 4cd6c7623517ee5347e7c68b0ea27effda781267 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Tue, 3 Dec 2024 23:25:49 +0700 Subject: [PATCH 04/45] feat: add subscriber for delta position --- include/suiryoku/locomotion/model/robot.hpp | 2 +- include/suiryoku/locomotion/node/locomotion_node.hpp | 2 ++ src/suiryoku/locomotion/model/robot.cpp | 9 ++------- src/suiryoku/locomotion/node/locomotion_node.cpp | 7 +++++++ 4 files changed, 12 insertions(+), 8 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 61475d4..bce5b42 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -74,7 +74,7 @@ class Robot bool is_calibrated; keisan::Angle orientation; keisan::Point2 position; - keisan::Point2 prev_position; + keisan::Point2 delta_position; bool is_walking; diff --git a/include/suiryoku/locomotion/node/locomotion_node.hpp b/include/suiryoku/locomotion/node/locomotion_node.hpp index d23a28a..2004449 100644 --- a/include/suiryoku/locomotion/node/locomotion_node.hpp +++ b/include/suiryoku/locomotion/node/locomotion_node.hpp @@ -73,6 +73,8 @@ class LocomotionNode rclcpp::Subscription::SharedPtr projected_objects_subscriber; + rclcpp::Subscription::SharedPtr delta_position_subscriber; + std::shared_ptr locomotion; std::shared_ptr robot; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 0d93b7f..bc4b62f 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -128,16 +128,11 @@ void Robot::resample_particles() void Robot::update_particles() { - double dx = position.x - prev_position.x; - double dy = position.y - prev_position.y; - for (int i = 0; i < num_particles; ++i) { - particles[i].position.x += dx; - particles[i].position.y += dy; + particles[i].position.x += delta_position.x; + particles[i].position.y += delta_position.y; particles[i].orientation = orientation; } - - prev_position = position; } double Robot::get_sum_weight() diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 68080c9..fbe52f3 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -78,6 +78,13 @@ LocomotionNode::LocomotionNode( this->robot->tilt = keisan::make_degree(message->tilt_angle); }); + delta_position_subscriber = node->create_subscription( + aruku::WalkingNode::delta_position_topic(), 10, + [this](const Point2::SharedPtr message) { + this->robot->delta_position.x = message->x; + this->robot->delta_position.y = message->y; + }); + projected_objects_subscriber = node->create_subscription( "/gyakuenki/projected_objects", 10, [this](const ProjectedObjects::SharedPtr message) { From a3e4132bbeeb3623790bcd6db1ab8babc0e6e8fa Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Thu, 5 Dec 2024 22:54:47 +0700 Subject: [PATCH 05/45] fix: localize every get delta pos msg --- src/suiryoku/locomotion/model/robot.cpp | 4 ++++ src/suiryoku/locomotion/node/locomotion_node.cpp | 6 ++---- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index bc4b62f..e8318b9 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -58,6 +58,10 @@ void Robot::localize() update_particles(); } + if (projected_objects.empty()) { + return; + } + calculate_weight(); resample_particles(); estimate_position(); diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index fbe52f3..5f4c5ca 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -65,10 +65,6 @@ LocomotionNode::LocomotionNode( this->robot->a_amplitude = message->a_amplitude; this->robot->position.x = message->odometry.x; this->robot->position.y = message->odometry.y; - - if (!this->robot->projected_objects.empty()) { - this->robot->localize(); - } }); head_subscriber = node->create_subscription( @@ -83,6 +79,8 @@ LocomotionNode::LocomotionNode( [this](const Point2::SharedPtr message) { this->robot->delta_position.x = message->x; this->robot->delta_position.y = message->y; + + this->robot->localize(); }); projected_objects_subscriber = node->create_subscription( From a333ac1764ff0d63774966b9f5b45e77fc565b53 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Thu, 5 Dec 2024 23:34:58 +0700 Subject: [PATCH 06/45] feat: add subscriber for button status --- include/suiryoku/locomotion/model/robot.hpp | 1 + .../locomotion/node/locomotion_node.hpp | 4 +++ src/suiryoku/locomotion/model/robot.cpp | 28 +++++++++++++++++++ .../locomotion/node/locomotion_node.cpp | 10 +++++++ 4 files changed, 43 insertions(+) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index bce5b42..b87ff33 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -57,6 +57,7 @@ class Robot void update_particles(); void calculate_weight(); void estimate_position(); + void print_particles(); double calculate_total_likelihood(const Particle & particle); double calculate_object_likelihood(const ProjectedObject & measurement, const Particle & particle); double get_sum_weight(); diff --git a/include/suiryoku/locomotion/node/locomotion_node.hpp b/include/suiryoku/locomotion/node/locomotion_node.hpp index 2004449..017e5e5 100644 --- a/include/suiryoku/locomotion/node/locomotion_node.hpp +++ b/include/suiryoku/locomotion/node/locomotion_node.hpp @@ -33,6 +33,7 @@ #include "rclcpp/rclcpp.hpp" #include "suiryoku/locomotion/model/robot.hpp" #include "suiryoku/locomotion/process/locomotion.hpp" +#include "tachimawari_interfaces/msg/status.hpp" namespace suiryoku { @@ -46,6 +47,7 @@ class LocomotionNode using SetWalking = aruku_interfaces::msg::SetWalking; using WalkingStatus = aruku_interfaces::msg::Status; using ProjectedObjects = gyakuenki_interfaces::msg::ProjectedObjects; + using TachimawariStatus = tachimawari_interfaces::msg::Status; static std::string get_node_prefix(); @@ -75,6 +77,8 @@ class LocomotionNode rclcpp::Subscription::SharedPtr delta_position_subscriber; + rclcpp::Subscription::SharedPtr button_status_subscriber; + std::shared_ptr locomotion; std::shared_ptr robot; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index e8318b9..87daf0a 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -220,4 +220,32 @@ void Robot::estimate_position() { estimated_position.y = y_mean; } +void Robot::print_particles() { + estimate_position(); + double sum_samples = 0.0; + + for (int i = 0; i < num_particles; i++) { + if (particles[i].weight > 0.0001) { + std::cout << "Particle " << std::setw(5) << i + << " weight: " << std::fixed << std::setprecision(5) + << particles[i].weight << std::setw(5) << " [" + << std::fixed << std::setprecision(2) << particles[i].position.x + << ", " << std::fixed << std::setprecision(2) + << particles[i].position.y << ", " << std::fixed + << std::setprecision(2) << particles[i].orientation.degree() << "]" + << std::endl; + + sum_samples += particles[i].weight; + } + } + + std::cout << "Num particles: " << num_particles << std::endl; + std::cout << "Sum weights: " << sum_samples << std::endl; + std::cout << "Pose estimation: " + << " [" << std::fixed << std::setprecision(2) + << estimated_position.x << ", " << std::fixed + << std::setprecision(2) << estimated_position.y + << "])" << std::endl; +} + } // namespace suiryoku diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 5f4c5ca..d3c2357 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -96,6 +96,16 @@ LocomotionNode::LocomotionNode( } }); + button_status_subscriber = node->create_subscription( + "/control/status", 10, + [this](const TachimawariStatus::SharedPtr message) { + if (message->button == 1) { + this->robot->kidnapped = false; + this->robot->init_particles(); + this->robot->print_particles(); + } + }); + locomotion->stop = [this]() {this->walking_state = false;}; locomotion->start = [this]() {this->walking_state = true;}; } From 8c8be987e667d155d1607e105f992be7d344b8f0 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 16 Dec 2024 23:46:10 +0700 Subject: [PATCH 07/45] feat: use landmark label to calculate likelihood --- CMakeLists.txt | 1 - include/suiryoku/locomotion/model/field.hpp | 49 ++++++++++++++++++--- src/suiryoku/locomotion/model/field.cpp | 18 -------- src/suiryoku/locomotion/model/robot.cpp | 17 +++++-- 4 files changed, 57 insertions(+), 28 deletions(-) delete mode 100644 src/suiryoku/locomotion/model/field.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9055286..e685cf8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,7 +35,6 @@ add_library(${PROJECT_NAME} SHARED "src/${PROJECT_NAME}/config/node/config_node.cpp" "src/${PROJECT_NAME}/locomotion/control/helper/parameter.cpp" "src/${PROJECT_NAME}/locomotion/control/node/control_node.cpp" - "src/${PROJECT_NAME}/locomotion/model/field.cpp" "src/${PROJECT_NAME}/locomotion/model/robot.cpp" "src/${PROJECT_NAME}/locomotion/node/locomotion_node.cpp" "src/${PROJECT_NAME}/locomotion/process/locomotion.cpp" diff --git a/include/suiryoku/locomotion/model/field.hpp b/include/suiryoku/locomotion/model/field.hpp index 70be43e..90c8574 100644 --- a/include/suiryoku/locomotion/model/field.hpp +++ b/include/suiryoku/locomotion/model/field.hpp @@ -8,15 +8,52 @@ namespace suiryoku { -class Field +struct Field { public: - Field(); + int width; + int length; + std::vector landmarks_L; + std::vector landmarks_T; + std::vector landmarks_X; + std::vector landmarks_goalpost; - const double width; - const double length; - const double num_landmarks; - std::vector landmarks; + Field() + : width(600), + length(900), + landmarks_L({ + {0.0, 0.0}, + {0.0, 600.0}, + {100.0, 50.0}, + {100.0, 550.0}, + {900.0, 0.0}, + {900.0, 600.0}, + {800.0, 50.0}, + {800.0, 550.0} + }), + landmarks_T({ + {0.0, 50.0}, + {0.0, 550.0}, + {450.0, 0.0}, + {450.0, 600.0}, + {900.0, 50.0}, + {900.0, 550.0} + }), + landmarks_X({ + {210, 300}, + {690, 300}, + {450, 300}, + {450, 375}, + {450, 225} + }), + landmarks_goalpost({ + {0.0, 170.0}, + {0.0, 430.0}, + {900.0, 170.0}, + {900.0, 430.0} + }) + { + } }; } // namespace suiryoku diff --git a/src/suiryoku/locomotion/model/field.cpp b/src/suiryoku/locomotion/model/field.cpp deleted file mode 100644 index efd8631..0000000 --- a/src/suiryoku/locomotion/model/field.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "suiryoku/locomotion/model/field.hpp" - -namespace suiryoku -{ - -Field::Field() -: width(600), length(900), num_landmarks(23), landmarks({ - {0.0, 0.0}, {0.0, 80.0}, {0.0, -80.0}, {0.0, 300.0}, - {0.0, -300.0}, {240.0, 0.0}, {350.0, 250.0}, {350.0, -250.0}, - {450.0, 250.0}, {450.0, -250.0}, {450.0, 300.0}, {450.0, -300.0}, - {450.0, 130.0}, {450.0, -130.0}, {-240.0, 0.0}, {-350.0, 250.0}, - {-350.0, -250.0}, {-450.0, 250.0}, {-450.0, -250.0}, {-450.0, 300.0}, - {-450.0, -300.0}, {-450.0, 130.0}, {-450.0, -130.0}, - }) -{ -} - -} // namespace suiryoku \ No newline at end of file diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 87daf0a..db0347b 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -179,12 +179,23 @@ double Robot::calculate_total_likelihood(const Particle & particle) { double Robot::calculate_object_likelihood( const ProjectedObject & measurement, const Particle & particle) { + std::vector landmarks; const double sigma_x = 1.0, sigma_y = 1.0; double relative_position_x, relative_position_y; double dx, dy, x_rot, y_rot, exponent, likelihood; double current_likelihood = 0.0; - for (int i = 0; i < field.num_landmarks; i++) { + if (measurement.label == "L-Intersection") { + landmarks = field.landmarks_L; + } else if (measurement.label == "T-Intersection") { + landmarks = field.landmarks_T; + } else if (measurement.label == "X-Intersection") { + landmarks = field.landmarks_X; + } else if (measurement.label == "goalpost") { + landmarks = field.landmarks_X; + } + + for (int i = 0; i < landmarks.size(); i++) { dx = measurement.center.x * 100; dy = measurement.center.y * 100; @@ -196,8 +207,8 @@ double Robot::calculate_object_likelihood( exponent = -0.5 * - (pow((field.landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + - pow((field.landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); + (pow((landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + + pow((landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y); From 69ee83dd0eb2e5d6db0919b6447695e328191213 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Wed, 18 Dec 2024 02:34:47 +0700 Subject: [PATCH 08/45] feat: update odometry use estimated pose --- include/suiryoku/locomotion/model/robot.hpp | 7 +- src/suiryoku/locomotion/model/robot.cpp | 66 ++++++++++++------- .../locomotion/node/locomotion_node.cpp | 5 +- 3 files changed, 50 insertions(+), 28 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index b87ff33..5fa41eb 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -54,13 +54,14 @@ class Robot void localize(); void init_particles(); void resample_particles(); - void update_particles(); + void update_motion(); void calculate_weight(); void estimate_position(); void print_particles(); double calculate_total_likelihood(const Particle & particle); double calculate_object_likelihood(const ProjectedObject & measurement, const Particle & particle); double get_sum_weight(); + bool get_apply_localization(); Field field; std::vector particles; @@ -94,6 +95,10 @@ class Robot double a_speed; bool aim_on; +private: + bool apply_localization; + double xvar; + double yvar; }; } // namespace suiryoku diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index db0347b..ebbe79c 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -33,9 +33,10 @@ namespace suiryoku Robot::Robot() : pan(0_deg), tilt(0_deg), pan_center(0_deg), tilt_center(0_deg), x_speed(0.0), - y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), - orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), - a_amplitude(0.0), is_calibrated(false), kidnapped(false), num_particles(0) + y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), orientation(0_deg), + position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), + is_calibrated(false), kidnapped(false), num_particles(0), apply_localization(false), + xvar(10.0), yvar(10.0) { } @@ -49,13 +50,17 @@ keisan::Angle Robot::get_tilt() const return tilt + tilt_center; } +bool Robot::get_apply_localization() { + return apply_localization; +} + void Robot::localize() { if (kidnapped) { init_particles(); kidnapped = false; } else { - update_particles(); + update_motion(); } if (projected_objects.empty()) { @@ -71,16 +76,14 @@ void Robot::init_particles() { particles.clear(); if (!kidnapped) { - const double var_x = 10.0, var_y = 10.0, var_w = 0.05; num_particles = 1000; - std::random_device xrd, yrd, wrd; - std::normal_distribution xrg(position.x, var_x), - yrg(position.y, var_y), wrg(orientation.degree(), var_w); + std::random_device xrd, yrd; + std::normal_distribution xrg(position.x, xvar), yrg(position.y, yvar); for (int i = 0; i < num_particles; ++i) { Particle new_particle; new_particle.position = keisan::Point2(xrg(xrd), yrg(yrd)); - new_particle.orientation = keisan::make_degree(wrg(wrd)).normalize(); + new_particle.orientation = orientation; new_particle.weight = 1.0 / num_particles; particles.push_back(new_particle); @@ -106,20 +109,17 @@ void Robot::resample_particles() { std::vector new_particles; std::random_device xrd, yrd, wrd; - const double var_x = 5.0, var_y = 5.0, var_w = 0.01; - for (auto & particle : particles) { - if (particle.weight >= 1.0 / (particles.size() * 10.0)) { - new_particles.push_back(particle); - std::normal_distribution xrg(particle.position.x, var_x), - yrg(particle.position.y, var_y), wrg(particle.orientation.degree(), var_w); - - int n = particle.weight * 100; + for (auto & p : particles) { + if (p.weight >= 1.0 / (particles.size() * 10.0)) { + new_particles.push_back(p); + std::normal_distribution xrg(p.position.x, xvar), yrg(p.position.y, yvar); + int n = p.weight * 100; for (int i = 0; i < n; ++i) { Particle new_particle; new_particle.position = keisan::Point2(xrg(xrd), yrg(yrd)); - new_particle.orientation = keisan::make_degree(wrg(wrd)).normalize(); + new_particle.orientation = orientation; new_particle.weight = 1.0 / n; new_particles.push_back(new_particle); @@ -130,12 +130,21 @@ void Robot::resample_particles() num_particles = particles.size(); } -void Robot::update_particles() +void Robot::update_motion() { - for (int i = 0; i < num_particles; ++i) { - particles[i].position.x += delta_position.x; - particles[i].position.y += delta_position.y; - particles[i].orientation = orientation; + static std::random_device xrd, yrd, wrd; + static std::normal_distribution<> xgen(0.0, xvar), ygen(0.0, yvar); + + for (auto & p : particles) { + double static_noise_x = xgen(xrd) / 5.0; + double static_noise_y = ygen(yrd) / 5.0; + double dynamic_noise_x = fabs(delta_position.x) * xgen(xrd) / 5.0; + double dynamic_noise_y = fabs(delta_position.y) * ygen(yrd) / 5.0; + double x_yterm = fabs(delta_position.y)*xgen(xrd) / 30.0; + double y_xterm = fabs(delta_position.x)*ygen(yrd) / 30.0; + p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; + p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; + p.orientation = orientation; } } @@ -196,8 +205,8 @@ double Robot::calculate_object_likelihood( } for (int i = 0; i < landmarks.size(); i++) { - dx = measurement.center.x * 100; - dy = measurement.center.y * 100; + dx = measurement.center.x; + dy = measurement.center.y; x_rot = dx * cos(particle.orientation.degree()) - dy * sin(particle.orientation.degree()); y_rot = dx * sin(particle.orientation.degree()) + dy * cos(particle.orientation.degree()); @@ -229,6 +238,13 @@ void Robot::estimate_position() { } estimated_position.x = x_mean; estimated_position.y = y_mean; + + // validate estimated position before assign to odometry + if (abs(estimated_position.x - position.x) < 20 && + abs(estimated_position.y - position.y) < 20) { + position = estimated_position; + apply_localization = true; + } } void Robot::print_particles() { diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index d3c2357..3fdad5c 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -96,13 +96,14 @@ LocomotionNode::LocomotionNode( } }); + // temporary button subscriber for testing button_status_subscriber = node->create_subscription( "/control/status", 10, [this](const TachimawariStatus::SharedPtr message) { if (message->button == 1) { this->robot->kidnapped = false; this->robot->init_particles(); - this->robot->print_particles(); + // this->robot->print_particles(); } }); @@ -113,7 +114,7 @@ LocomotionNode::LocomotionNode( void LocomotionNode::update() { publish_walking(); - if (set_odometry) { + if (set_odometry || this->robot->get_apply_localization()) { publish_odometry(); } } From e57ca4bd32690405bd28684268b71f44d6eb4271 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Fri, 20 Dec 2024 03:48:50 +0700 Subject: [PATCH 09/45] feat: debug log --- include/suiryoku/locomotion/model/robot.hpp | 6 +- src/suiryoku/locomotion/model/robot.cpp | 56 ++++++++++++------- .../locomotion/node/locomotion_node.cpp | 9 ++- 3 files changed, 47 insertions(+), 24 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 5fa41eb..077cc6b 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -49,6 +49,9 @@ class Robot keisan::Angle get_pan() const; keisan::Angle get_tilt() const; + bool get_apply_localization(); + void set_apply_localization(bool apply_localization); + void set_initial_localization(bool initial_localization); // localizations void localize(); @@ -61,12 +64,10 @@ class Robot double calculate_total_likelihood(const Particle & particle); double calculate_object_likelihood(const ProjectedObject & measurement, const Particle & particle); double get_sum_weight(); - bool get_apply_localization(); Field field; std::vector particles; int num_particles; - bool kidnapped; keisan::Point2 estimated_position; // IPM @@ -97,6 +98,7 @@ class Robot private: bool apply_localization; + bool initial_localization; double xvar; double yvar; }; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index ebbe79c..b279929 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -35,8 +35,8 @@ Robot::Robot() : pan(0_deg), tilt(0_deg), pan_center(0_deg), tilt_center(0_deg), x_speed(0.0), y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), - is_calibrated(false), kidnapped(false), num_particles(0), apply_localization(false), - xvar(10.0), yvar(10.0) + is_calibrated(false), num_particles(0), apply_localization(false), + initial_localization(true), xvar(10.0), yvar(10.0) { } @@ -54,28 +54,43 @@ bool Robot::get_apply_localization() { return apply_localization; } +void Robot::set_apply_localization(bool apply_localization) { + this->apply_localization = apply_localization; +} + +void Robot::set_initial_localization(bool initial_localization) { + this->initial_localization = initial_localization; +} + void Robot::localize() { - if (kidnapped) { + if (num_particles == 0) { init_particles(); - kidnapped = false; } else { + std::cout << "update motion" << std::endl; update_motion(); } + print_particles(); + if (projected_objects.empty()) { + std::cout << "not receive projected objects" << std::endl; return; } + std::cout << "calculate weight" << std::endl; calculate_weight(); + std::cout << "resample particles" << std::endl; resample_particles(); + std::cout << "estimate_position" << std::endl; estimate_position(); } void Robot::init_particles() { particles.clear(); - if (!kidnapped) { + if (initial_localization) { + initial_localization = false; num_particles = 1000; std::random_device xrd, yrd; std::normal_distribution xrg(position.x, xvar), yrg(position.y, yvar); @@ -88,7 +103,7 @@ void Robot::init_particles() particles.push_back(new_particle); } - } else { // if kidnapped, generate particles all over the field + } else { // if not initial, generate particles all over the field const int x_gap = 5, y_gap = 5; num_particles = field.width * field.length / (x_gap * y_gap); @@ -151,28 +166,27 @@ void Robot::update_motion() double Robot::get_sum_weight() { double sum_weight = 0.0; - for (int i = 0; i < num_particles; ++i) { - sum_weight += particles[i].weight; + for (auto & p : particles) { + sum_weight += p.weight; } return sum_weight; } void Robot::calculate_weight() { - for (int i = 0; i < num_particles; ++i) { - double likelihood = calculate_total_likelihood(particles[i]); - particles[i].weight = likelihood; + for (auto & p : particles) { + double likelihood = calculate_total_likelihood(p); + p.weight = likelihood; } double sum_weight = get_sum_weight(); if (sum_weight > 0.0) { - for (int i = 0; i < num_particles; ++i) { - particles[i].weight /= sum_weight; + for (auto & p : particles) { + p.weight /= sum_weight; } } else { - for (int i = 0; i < num_particles; ++i) { - particles[i].weight = 1 / num_particles; - } + initial_localization = true; + init_particles(); } } @@ -189,7 +203,7 @@ double Robot::calculate_total_likelihood(const Particle & particle) { double Robot::calculate_object_likelihood( const ProjectedObject & measurement, const Particle & particle) { std::vector landmarks; - const double sigma_x = 1.0, sigma_y = 1.0; + double sigma_x = 1.0, sigma_y = 1.0; double relative_position_x, relative_position_y; double dx, dy, x_rot, y_rot, exponent, likelihood; double current_likelihood = 0.0; @@ -205,8 +219,8 @@ double Robot::calculate_object_likelihood( } for (int i = 0; i < landmarks.size(); i++) { - dx = measurement.center.x; - dy = measurement.center.y; + dx = measurement.center.x * 100; + dy = measurement.center.y * 100; x_rot = dx * cos(particle.orientation.degree()) - dy * sin(particle.orientation.degree()); y_rot = dx * sin(particle.orientation.degree()) + dy * cos(particle.orientation.degree()); @@ -230,6 +244,10 @@ double Robot::calculate_object_likelihood( } void Robot::estimate_position() { + if (num_particles == 0 || get_sum_weight() == 0) { + return; + } + double x_mean = 0.0; double y_mean = 0.0; for (auto p : particles) { diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 3fdad5c..9bad93b 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -93,17 +93,18 @@ LocomotionNode::LocomotionNode( obj.label, keisan::Point3{obj.center.x, obj.center.y, obj.center.z} }); + std::cout << "receive projected_objests" << std::endl; } }); // temporary button subscriber for testing button_status_subscriber = node->create_subscription( - "/control/status", 10, + "/measurement/button_status", 10, [this](const TachimawariStatus::SharedPtr message) { if (message->button == 1) { - this->robot->kidnapped = false; + this->robot->set_initial_localization(true); this->robot->init_particles(); - // this->robot->print_particles(); + this->robot->print_particles(); } }); @@ -116,6 +117,8 @@ void LocomotionNode::update() publish_walking(); if (set_odometry || this->robot->get_apply_localization()) { publish_odometry(); + this->robot->set_apply_localization(false); + std::cout << "odometry_updated" << std::endl; } } From b942ba360cfa7c98b3313297b51ebbec8839f4df Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Thu, 26 Dec 2024 01:22:20 +0700 Subject: [PATCH 10/45] feat: debug and testing log --- include/suiryoku/locomotion/model/robot.hpp | 1 + src/suiryoku/locomotion/model/robot.cpp | 34 ++++++++++++------- .../locomotion/node/locomotion_node.cpp | 23 +++++++------ 3 files changed, 36 insertions(+), 22 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 077cc6b..0905fc3 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -61,6 +61,7 @@ class Robot void calculate_weight(); void estimate_position(); void print_particles(); + void print_estimate_position(); double calculate_total_likelihood(const Particle & particle); double calculate_object_likelihood(const ProjectedObject & measurement, const Particle & particle); double get_sum_weight(); diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index b279929..252e607 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -64,14 +64,17 @@ void Robot::set_initial_localization(bool initial_localization) { void Robot::localize() { - if (num_particles == 0) { + if (num_particles == 0 || initial_localization) { init_particles(); } else { std::cout << "update motion" << std::endl; update_motion(); } - print_particles(); + // print_particles(); + std::cout << "estimate_position" << std::endl; + estimate_position(); + print_estimate_position(); if (projected_objects.empty()) { std::cout << "not receive projected objects" << std::endl; @@ -82,16 +85,16 @@ void Robot::localize() calculate_weight(); std::cout << "resample particles" << std::endl; resample_particles(); - std::cout << "estimate_position" << std::endl; - estimate_position(); } void Robot::init_particles() { particles.clear(); if (initial_localization) { + std::cout << "INIT PARTICLES BASED ON INITIAL POSE" << std::endl; + initial_localization = false; - num_particles = 1000; + num_particles = 250; std::random_device xrd, yrd; std::normal_distribution xrg(position.x, xvar), yrg(position.y, yvar); @@ -104,6 +107,8 @@ void Robot::init_particles() particles.push_back(new_particle); } } else { // if not initial, generate particles all over the field + std::cout << "INIT PARTICLES ALL OVER FIELD" << std::endl; + const int x_gap = 5, y_gap = 5; num_particles = field.width * field.length / (x_gap * y_gap); @@ -257,16 +262,11 @@ void Robot::estimate_position() { estimated_position.x = x_mean; estimated_position.y = y_mean; - // validate estimated position before assign to odometry - if (abs(estimated_position.x - position.x) < 20 && - abs(estimated_position.y - position.y) < 20) { - position = estimated_position; - apply_localization = true; - } + position = estimated_position; + apply_localization = true; } void Robot::print_particles() { - estimate_position(); double sum_samples = 0.0; for (int i = 0; i < num_particles; i++) { @@ -293,4 +293,14 @@ void Robot::print_particles() { << "])" << std::endl; } +void Robot::print_estimate_position() { + std::cout << "Num particles: " << num_particles << std::endl; + std::cout << "Sum weights: " << get_sum_weight() << std::endl; + std::cout << "Pose estimation: " + << " [" << std::fixed << std::setprecision(2) + << estimated_position.x << ", " << std::fixed + << std::setprecision(2) << estimated_position.y + << "])" << std::endl; +} + } // namespace suiryoku diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 9bad93b..6633851 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -98,15 +98,15 @@ LocomotionNode::LocomotionNode( }); // temporary button subscriber for testing - button_status_subscriber = node->create_subscription( - "/measurement/button_status", 10, - [this](const TachimawariStatus::SharedPtr message) { - if (message->button == 1) { - this->robot->set_initial_localization(true); - this->robot->init_particles(); - this->robot->print_particles(); - } - }); + // button_status_subscriber = node->create_subscription( + // "/measurement/button_status", 10, + // [this](const TachimawariStatus::SharedPtr message) { + // if (message->button == 1) { + // this->robot->set_initial_localization(true); + // this->robot->init_particles(); + // this->robot->print_estimate_position(); + // } + // }); locomotion->stop = [this]() {this->walking_state = false;}; locomotion->start = [this]() {this->walking_state = true;}; @@ -115,8 +115,11 @@ LocomotionNode::LocomotionNode( void LocomotionNode::update() { publish_walking(); - if (set_odometry || this->robot->get_apply_localization()) { + if (set_odometry) { publish_odometry(); + } + + if (this->robot->get_apply_localization()) { this->robot->set_apply_localization(false); std::cout << "odometry_updated" << std::endl; } From 58d2418fea4da7ba96e917c84b26c4bba3122132 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 5 Jan 2025 03:30:46 +0700 Subject: [PATCH 11/45] fix: change attribute name --- include/suiryoku/locomotion/model/robot.hpp | 2 +- src/suiryoku/locomotion/model/robot.cpp | 12 +++++++++--- src/suiryoku/locomotion/node/locomotion_node.cpp | 4 ++-- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 0905fc3..51f2771 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -32,7 +32,7 @@ namespace suiryoku struct ProjectedObject { std::string label; - keisan::Point3 center; + keisan::Point3 position; }; struct Particle diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 252e607..017aa18 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -92,9 +92,10 @@ void Robot::init_particles() particles.clear(); if (initial_localization) { std::cout << "INIT PARTICLES BASED ON INITIAL POSE" << std::endl; + std::cout << "position: " << position.x << ", " << position.y << std::endl; initial_localization = false; - num_particles = 250; + num_particles = 1000; std::random_device xrd, yrd; std::normal_distribution xrg(position.x, xvar), yrg(position.y, yvar); @@ -165,6 +166,10 @@ void Robot::update_motion() p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; p.orientation = orientation; + + // p.position.x += delta_position.x; + // p.position.y += delta_position.y; + // p.orientation = orientation; } } @@ -190,6 +195,7 @@ void Robot::calculate_weight() p.weight /= sum_weight; } } else { + std::cout << "SUM WEIGHT = 0" << std::endl; initial_localization = true; init_particles(); } @@ -224,8 +230,8 @@ double Robot::calculate_object_likelihood( } for (int i = 0; i < landmarks.size(); i++) { - dx = measurement.center.x * 100; - dy = measurement.center.y * 100; + dx = measurement.position.x * 100; + dy = measurement.position.y * 100; x_rot = dx * cos(particle.orientation.degree()) - dy * sin(particle.orientation.degree()); y_rot = dx * sin(particle.orientation.degree()) + dy * cos(particle.orientation.degree()); diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 6633851..e6c662f 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -91,9 +91,9 @@ LocomotionNode::LocomotionNode( this->robot->projected_objects.push_back( ProjectedObject{ obj.label, - keisan::Point3{obj.center.x, obj.center.y, obj.center.z} + keisan::Point3{obj.position.x, obj.position.y, obj.position.z} }); - std::cout << "receive projected_objests" << std::endl; + // std::cout << "receive projected_objests" << std::endl; } }); From 8c9f1b2cfd7ed0007388a2d5dd994d148ccf81c2 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Tue, 7 Jan 2025 17:10:19 +0700 Subject: [PATCH 12/45] fix: apply estimate pose --- .../locomotion/node/locomotion_node.cpp | 20 ++----------------- 1 file changed, 2 insertions(+), 18 deletions(-) diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index e6c662f..b169239 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -93,21 +93,9 @@ LocomotionNode::LocomotionNode( obj.label, keisan::Point3{obj.position.x, obj.position.y, obj.position.z} }); - // std::cout << "receive projected_objests" << std::endl; } }); - // temporary button subscriber for testing - // button_status_subscriber = node->create_subscription( - // "/measurement/button_status", 10, - // [this](const TachimawariStatus::SharedPtr message) { - // if (message->button == 1) { - // this->robot->set_initial_localization(true); - // this->robot->init_particles(); - // this->robot->print_estimate_position(); - // } - // }); - locomotion->stop = [this]() {this->walking_state = false;}; locomotion->start = [this]() {this->walking_state = true;}; } @@ -115,13 +103,9 @@ LocomotionNode::LocomotionNode( void LocomotionNode::update() { publish_walking(); - if (set_odometry) { - publish_odometry(); - } - - if (this->robot->get_apply_localization()) { + if (set_odometry || this->robot->get_apply_localization()) { this->robot->set_apply_localization(false); - std::cout << "odometry_updated" << std::endl; + publish_odometry(); } } From 7f39056c83234b75f2391fa6a133f397854bbd07 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Tue, 7 Jan 2025 18:22:18 +0700 Subject: [PATCH 13/45] refactor: remove debug logs --- src/suiryoku/locomotion/model/robot.cpp | 15 +-------------- src/suiryoku/locomotion/node/locomotion_node.cpp | 2 +- 2 files changed, 2 insertions(+), 15 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 017aa18..088ac25 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -67,23 +67,18 @@ void Robot::localize() if (num_particles == 0 || initial_localization) { init_particles(); } else { - std::cout << "update motion" << std::endl; update_motion(); } // print_particles(); - std::cout << "estimate_position" << std::endl; estimate_position(); print_estimate_position(); if (projected_objects.empty()) { - std::cout << "not receive projected objects" << std::endl; return; } - std::cout << "calculate weight" << std::endl; calculate_weight(); - std::cout << "resample particles" << std::endl; resample_particles(); } @@ -92,7 +87,6 @@ void Robot::init_particles() particles.clear(); if (initial_localization) { std::cout << "INIT PARTICLES BASED ON INITIAL POSE" << std::endl; - std::cout << "position: " << position.x << ", " << position.y << std::endl; initial_localization = false; num_particles = 1000; @@ -195,7 +189,6 @@ void Robot::calculate_weight() p.weight /= sum_weight; } } else { - std::cout << "SUM WEIGHT = 0" << std::endl; initial_localization = true; init_particles(); } @@ -292,16 +285,10 @@ void Robot::print_particles() { std::cout << "Num particles: " << num_particles << std::endl; std::cout << "Sum weights: " << sum_samples << std::endl; - std::cout << "Pose estimation: " - << " [" << std::fixed << std::setprecision(2) - << estimated_position.x << ", " << std::fixed - << std::setprecision(2) << estimated_position.y - << "])" << std::endl; + print_estimate_position(); } void Robot::print_estimate_position() { - std::cout << "Num particles: " << num_particles << std::endl; - std::cout << "Sum weights: " << get_sum_weight() << std::endl; std::cout << "Pose estimation: " << " [" << std::fixed << std::setprecision(2) << estimated_position.x << ", " << std::fixed diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index b169239..57ebb35 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -84,7 +84,7 @@ LocomotionNode::LocomotionNode( }); projected_objects_subscriber = node->create_subscription( - "/gyakuenki/projected_objects", 10, + "/gyakuenki_cpp/projected_objects", 10, [this](const ProjectedObjects::SharedPtr message) { this->robot->projected_objects.clear(); for (const auto & obj : message->projected_objects) { From faef1d4fe9ce5d99bd474a58de0b1e9fc51408e5 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Tue, 7 Jan 2025 20:17:51 +0700 Subject: [PATCH 14/45] feat: use counter to initialize particles across the field --- include/suiryoku/locomotion/model/robot.hpp | 1 + src/suiryoku/locomotion/model/robot.cpp | 15 ++++++++++----- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 51f2771..21f13f1 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -102,6 +102,7 @@ class Robot bool initial_localization; double xvar; double yvar; + int kidnap_counter; }; } // namespace suiryoku diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 088ac25..db295b9 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -36,7 +36,7 @@ Robot::Robot() y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), is_calibrated(false), num_particles(0), apply_localization(false), - initial_localization(true), xvar(10.0), yvar(10.0) + initial_localization(true), xvar(10.0), yvar(10.0), kidnap_counter(0) { } @@ -86,7 +86,7 @@ void Robot::init_particles() { particles.clear(); if (initial_localization) { - std::cout << "INIT PARTICLES BASED ON INITIAL POSE" << std::endl; + std::cout << "INIT PARTICLES BASED ON LAST POSE" << std::endl; initial_localization = false; num_particles = 1000; @@ -117,6 +117,7 @@ void Robot::init_particles() particles.push_back(new_particle); } } + kidnap_counter = 0; } } @@ -188,8 +189,12 @@ void Robot::calculate_weight() for (auto & p : particles) { p.weight /= sum_weight; } + kidnap_counter = 0; } else { - initial_localization = true; + kidnap_counter++; + if (kidnap_counter < 5) { + initial_localization = true; + } init_particles(); } } @@ -258,8 +263,8 @@ void Robot::estimate_position() { x_mean += (1.0 / num_particles) * p.position.x; y_mean += (1.0 / num_particles) * p.position.y; } - estimated_position.x = x_mean; - estimated_position.y = y_mean; + estimated_position.x = (x_mean < 0.0) ? 0.0 : (x_mean > 900.0 ? 900.0 : x_mean); + estimated_position.y = (y_mean < 0.0) ? 0.0 : (y_mean > 600.0 ? 600.0 : y_mean); position = estimated_position; apply_localization = true; From 06d20e272ec9014cd34eee4ed393ef10b80661aa Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Tue, 7 Jan 2025 20:45:24 +0700 Subject: [PATCH 15/45] fix: clear projected objects after evaluate particles --- src/suiryoku/locomotion/model/robot.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index db295b9..6406c9a 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -104,7 +104,7 @@ void Robot::init_particles() } else { // if not initial, generate particles all over the field std::cout << "INIT PARTICLES ALL OVER FIELD" << std::endl; - const int x_gap = 5, y_gap = 5; + const int x_gap = 10, y_gap = 10; num_particles = field.width * field.length / (x_gap * y_gap); for (int i = -field.width / 2; i < field.width / 2; i += x_gap) { @@ -191,12 +191,13 @@ void Robot::calculate_weight() } kidnap_counter = 0; } else { - kidnap_counter++; - if (kidnap_counter < 5) { + if (kidnap_counter++ < 1) { initial_localization = true; } init_particles(); } + + projected_objects.clear(); } double Robot::calculate_total_likelihood(const Particle & particle) { From 7f253d9425ab20b3a51026f2e827f1c1ed2564ea Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Thu, 9 Jan 2025 23:01:27 +0700 Subject: [PATCH 16/45] feat: validate delta pose before localize --- src/suiryoku/locomotion/node/locomotion_node.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 57ebb35..91e8551 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -80,7 +80,9 @@ LocomotionNode::LocomotionNode( this->robot->delta_position.x = message->x; this->robot->delta_position.y = message->y; - this->robot->localize(); + if (message->x != 0.0 || message->y != 0.0 || this->robot->a_speed != 0.0) { + this->robot->localize(); + } }); projected_objects_subscriber = node->create_subscription( From 81bd7634300dbfe3e9035721fbdc88f432d17ae8 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Fri, 10 Jan 2025 01:13:46 +0700 Subject: [PATCH 17/45] refactor: pass initial localization as method parameter --- include/suiryoku/locomotion/model/robot.hpp | 6 ++---- src/suiryoku/locomotion/model/robot.cpp | 19 +++++++------------ 2 files changed, 9 insertions(+), 16 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 21f13f1..3f75cee 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -51,11 +51,10 @@ class Robot keisan::Angle get_tilt() const; bool get_apply_localization(); void set_apply_localization(bool apply_localization); - void set_initial_localization(bool initial_localization); // localizations - void localize(); - void init_particles(); + void localize(bool initial_localization = false); + void init_particles(bool initial_localization); void resample_particles(); void update_motion(); void calculate_weight(); @@ -99,7 +98,6 @@ class Robot private: bool apply_localization; - bool initial_localization; double xvar; double yvar; int kidnap_counter; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 6406c9a..08c9856 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -36,7 +36,7 @@ Robot::Robot() y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), is_calibrated(false), num_particles(0), apply_localization(false), - initial_localization(true), xvar(10.0), yvar(10.0), kidnap_counter(0) + xvar(10.0), yvar(10.0), kidnap_counter(0) { } @@ -58,19 +58,14 @@ void Robot::set_apply_localization(bool apply_localization) { this->apply_localization = apply_localization; } -void Robot::set_initial_localization(bool initial_localization) { - this->initial_localization = initial_localization; -} - -void Robot::localize() +void Robot::localize(bool initial_localization) { if (num_particles == 0 || initial_localization) { - init_particles(); + init_particles(initial_localization); } else { update_motion(); } - // print_particles(); estimate_position(); print_estimate_position(); @@ -82,13 +77,12 @@ void Robot::localize() resample_particles(); } -void Robot::init_particles() +void Robot::init_particles(bool initial_localization) { particles.clear(); if (initial_localization) { std::cout << "INIT PARTICLES BASED ON LAST POSE" << std::endl; - initial_localization = false; num_particles = 1000; std::random_device xrd, yrd; std::normal_distribution xrg(position.x, xvar), yrg(position.y, yvar); @@ -192,9 +186,10 @@ void Robot::calculate_weight() kidnap_counter = 0; } else { if (kidnap_counter++ < 1) { - initial_localization = true; + init_particles(true); + } else { + init_particles(false); } - init_particles(); } projected_objects.clear(); From 0eed5efd88716bab8d48c603773c42f1f0b22096 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sat, 11 Jan 2025 13:21:54 +0700 Subject: [PATCH 18/45] docs: add copyrights --- include/suiryoku/locomotion/model/field.hpp | 22 ++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/include/suiryoku/locomotion/model/field.hpp b/include/suiryoku/locomotion/model/field.hpp index 90c8574..9e75fb6 100644 --- a/include/suiryoku/locomotion/model/field.hpp +++ b/include/suiryoku/locomotion/model/field.hpp @@ -1,3 +1,23 @@ +// Copyright (c) 2025 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + #ifndef SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_ #define SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_ @@ -58,4 +78,4 @@ struct Field } // namespace suiryoku -#endif // SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_ \ No newline at end of file +#endif // SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_ From 38fe6267652682cac6eaed090185095bf6ddae5a Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sat, 11 Jan 2025 14:02:36 +0700 Subject: [PATCH 19/45] refactor: clean up code --- include/suiryoku/locomotion/model/robot.hpp | 6 ++---- .../locomotion/node/locomotion_node.hpp | 6 +----- src/suiryoku/locomotion/model/robot.cpp | 18 +++--------------- .../locomotion/node/locomotion_node.cpp | 4 ++-- 4 files changed, 8 insertions(+), 26 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 3f75cee..49dca6c 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -49,8 +49,6 @@ class Robot keisan::Angle get_pan() const; keisan::Angle get_tilt() const; - bool get_apply_localization(); - void set_apply_localization(bool apply_localization); // localizations void localize(bool initial_localization = false); @@ -67,8 +65,9 @@ class Robot Field field; std::vector particles; - int num_particles; keisan::Point2 estimated_position; + int num_particles; + bool apply_localization; // IPM std::vector projected_objects; @@ -97,7 +96,6 @@ class Robot bool aim_on; private: - bool apply_localization; double xvar; double yvar; int kidnap_counter; diff --git a/include/suiryoku/locomotion/node/locomotion_node.hpp b/include/suiryoku/locomotion/node/locomotion_node.hpp index 017e5e5..7ad954a 100644 --- a/include/suiryoku/locomotion/node/locomotion_node.hpp +++ b/include/suiryoku/locomotion/node/locomotion_node.hpp @@ -33,7 +33,6 @@ #include "rclcpp/rclcpp.hpp" #include "suiryoku/locomotion/model/robot.hpp" #include "suiryoku/locomotion/process/locomotion.hpp" -#include "tachimawari_interfaces/msg/status.hpp" namespace suiryoku { @@ -44,10 +43,9 @@ class LocomotionNode using Head = atama_interfaces::msg::Head; using MeasurementStatus = kansei_interfaces::msg::Status; using Point2 = aruku_interfaces::msg::Point2; + using ProjectedObjects = gyakuenki_interfaces::msg::ProjectedObjects; using SetWalking = aruku_interfaces::msg::SetWalking; using WalkingStatus = aruku_interfaces::msg::Status; - using ProjectedObjects = gyakuenki_interfaces::msg::ProjectedObjects; - using TachimawariStatus = tachimawari_interfaces::msg::Status; static std::string get_node_prefix(); @@ -77,8 +75,6 @@ class LocomotionNode rclcpp::Subscription::SharedPtr delta_position_subscriber; - rclcpp::Subscription::SharedPtr button_status_subscriber; - std::shared_ptr locomotion; std::shared_ptr robot; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 08c9856..e92987c 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -50,14 +50,6 @@ keisan::Angle Robot::get_tilt() const return tilt + tilt_center; } -bool Robot::get_apply_localization() { - return apply_localization; -} - -void Robot::set_apply_localization(bool apply_localization) { - this->apply_localization = apply_localization; -} - void Robot::localize(bool initial_localization) { if (num_particles == 0 || initial_localization) { @@ -120,7 +112,7 @@ void Robot::resample_particles() std::vector new_particles; std::random_device xrd, yrd, wrd; - for (auto & p : particles) { + for (const auto & p : particles) { if (p.weight >= 1.0 / (particles.size() * 10.0)) { new_particles.push_back(p); std::normal_distribution xrg(p.position.x, xvar), yrg(p.position.y, yvar); @@ -155,17 +147,13 @@ void Robot::update_motion() p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; p.orientation = orientation; - - // p.position.x += delta_position.x; - // p.position.y += delta_position.y; - // p.orientation = orientation; } } double Robot::get_sum_weight() { double sum_weight = 0.0; - for (auto & p : particles) { + for (const auto & p : particles) { sum_weight += p.weight; } return sum_weight; @@ -255,7 +243,7 @@ void Robot::estimate_position() { double x_mean = 0.0; double y_mean = 0.0; - for (auto p : particles) { + for (const auto & p : particles) { x_mean += (1.0 / num_particles) * p.position.x; y_mean += (1.0 / num_particles) * p.position.y; } diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 91e8551..812e04b 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -105,8 +105,8 @@ LocomotionNode::LocomotionNode( void LocomotionNode::update() { publish_walking(); - if (set_odometry || this->robot->get_apply_localization()) { - this->robot->set_apply_localization(false); + if (set_odometry || this->robot->apply_localization) { + this->robot->apply_localization = false; publish_odometry(); } } From 94f3573812992722350c88497c9aa8f29082c0d8 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sat, 11 Jan 2025 14:08:24 +0700 Subject: [PATCH 20/45] fix: change particles initialization range --- src/suiryoku/locomotion/model/robot.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index e92987c..d1375c0 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -93,8 +93,8 @@ void Robot::init_particles(bool initial_localization) const int x_gap = 10, y_gap = 10; num_particles = field.width * field.length / (x_gap * y_gap); - for (int i = -field.width / 2; i < field.width / 2; i += x_gap) { - for (int j = -field.length / 2; j < field.length / 2; j += y_gap) { + for (int i = 0; i <= field.width; i += x_gap) { + for (int j = 0; j <= field.length; j += y_gap) { Particle new_particle; new_particle.position = keisan::Point2(i, j); new_particle.orientation = orientation; From cfd0ec8bca8f95fc546d9a52441fe9bdd9e8858f Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sat, 11 Jan 2025 21:40:03 +0700 Subject: [PATCH 21/45] fix: swap field width and length in init particles --- src/suiryoku/locomotion/model/robot.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index d1375c0..32efefb 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -93,8 +93,8 @@ void Robot::init_particles(bool initial_localization) const int x_gap = 10, y_gap = 10; num_particles = field.width * field.length / (x_gap * y_gap); - for (int i = 0; i <= field.width; i += x_gap) { - for (int j = 0; j <= field.length; j += y_gap) { + for (int i = 0; i <= field.length; i += x_gap) { + for (int j = 0; j <= field.width; j += y_gap) { Particle new_particle; new_particle.position = keisan::Point2(i, j); new_particle.orientation = orientation; From 99384771afd7b00906ab120ea9a2fe577540922d Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 12 Jan 2025 00:07:05 +0700 Subject: [PATCH 22/45] feat: validate estimated pose before apply using num particles --- src/suiryoku/locomotion/model/robot.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 32efefb..9abc7e5 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -250,8 +250,10 @@ void Robot::estimate_position() { estimated_position.x = (x_mean < 0.0) ? 0.0 : (x_mean > 900.0 ? 900.0 : x_mean); estimated_position.y = (y_mean < 0.0) ? 0.0 : (y_mean > 600.0 ? 600.0 : y_mean); - position = estimated_position; - apply_localization = true; + if (num_particles < 1000) { + position = estimated_position; + apply_localization = true; + } } void Robot::print_particles() { From 5da59edcbfcadc8da59de203af51ab29191a9e1a Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 12 Jan 2025 01:04:22 +0700 Subject: [PATCH 23/45] fix: use radian for trigonometri calculation --- src/suiryoku/locomotion/model/robot.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 9abc7e5..34006c5 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -58,15 +58,15 @@ void Robot::localize(bool initial_localization) update_motion(); } - estimate_position(); - print_estimate_position(); - if (projected_objects.empty()) { return; } calculate_weight(); resample_particles(); + + estimate_position(); + print_estimate_position(); } void Robot::init_particles(bool initial_localization) @@ -162,8 +162,7 @@ double Robot::get_sum_weight() void Robot::calculate_weight() { for (auto & p : particles) { - double likelihood = calculate_total_likelihood(p); - p.weight = likelihood; + p.weight = calculate_total_likelihood(p); } double sum_weight = get_sum_weight(); @@ -215,8 +214,8 @@ double Robot::calculate_object_likelihood( dx = measurement.position.x * 100; dy = measurement.position.y * 100; - x_rot = dx * cos(particle.orientation.degree()) - dy * sin(particle.orientation.degree()); - y_rot = dx * sin(particle.orientation.degree()) + dy * cos(particle.orientation.degree()); + x_rot = dx * cos(particle.orientation.radian()) - dy * sin(particle.orientation.radian()); + y_rot = dx * sin(particle.orientation.radian()) + dy * cos(particle.orientation.radian()); relative_position_x = particle.position.x + x_rot; relative_position_y = particle.position.y + y_rot; From 01b85a13bb254c3dc2193a0f1b321117e905f386 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 13 Jan 2025 12:44:16 +0700 Subject: [PATCH 24/45] feat: add option to enable localization in config --- include/suiryoku/locomotion/model/robot.hpp | 2 ++ .../suiryoku/locomotion/process/locomotion.hpp | 2 ++ src/suiryoku/locomotion/model/robot.cpp | 4 ++-- src/suiryoku/locomotion/node/locomotion_node.cpp | 3 ++- src/suiryoku/locomotion/process/locomotion.cpp | 15 +++++++++++++++ 5 files changed, 23 insertions(+), 3 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 49dca6c..27bd65c 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -67,6 +67,8 @@ class Robot std::vector particles; keisan::Point2 estimated_position; int num_particles; + + bool use_localization; bool apply_localization; // IPM diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index e12dd0a..cde91b1 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -167,6 +167,8 @@ class Locomotion keisan::Angle right_kick_target_pan; keisan::Angle right_kick_target_tilt; + bool localization_enable; + std::shared_ptr robot; }; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 34006c5..15f3d8b 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -35,8 +35,8 @@ Robot::Robot() : pan(0_deg), tilt(0_deg), pan_center(0_deg), tilt_center(0_deg), x_speed(0.0), y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), - is_calibrated(false), num_particles(0), apply_localization(false), - xvar(10.0), yvar(10.0), kidnap_counter(0) + is_calibrated(false), use_localization(false), apply_localization(false), + num_particles(0), xvar(10.0), yvar(10.0), kidnap_counter(0) { } diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 812e04b..eb95a20 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -80,7 +80,8 @@ LocomotionNode::LocomotionNode( this->robot->delta_position.x = message->x; this->robot->delta_position.y = message->y; - if (message->x != 0.0 || message->y != 0.0 || this->robot->a_speed != 0.0) { + if (this->robot->use_localization && + (message->x || message->y || this->robot->a_speed)) { this->robot->localize(); } }); diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index aaa40b2..eca33d8 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -287,6 +287,21 @@ void Locomotion::set_config(const nlohmann::json & json) valid_config = false; } + nlohmann::json localization_section; + if (jitsuyo::assign_val(json, "localization", localization_section)) { + bool valid_section = true; + + valid_section &= jitsuyo::assign_val(localization_section, "enable", localization_enable); + robot->use_localization = localization_enable; + + if (!valid_section) { + std::cout << "Error found at section `localization`" << std::endl; + valid_config = false; + } + } else { + valid_config = false; + } + if (!valid_config) { throw std::runtime_error("Failed to load config file `locomotion.json`"); } From b0f7c528657cf4c808e51c0ff74aa260c50e046b Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 24 Feb 2025 13:58:31 +0700 Subject: [PATCH 25/45] fix: improve loop code --- src/suiryoku/locomotion/model/robot.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 15f3d8b..2f3a049 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -79,19 +79,22 @@ void Robot::init_particles(bool initial_localization) std::random_device xrd, yrd; std::normal_distribution xrg(position.x, xvar), yrg(position.y, yvar); + particles.resize(num_particles); for (int i = 0; i < num_particles; ++i) { Particle new_particle; new_particle.position = keisan::Point2(xrg(xrd), yrg(yrd)); new_particle.orientation = orientation; new_particle.weight = 1.0 / num_particles; - particles.push_back(new_particle); + particles[i] = new_particle; } } else { // if not initial, generate particles all over the field std::cout << "INIT PARTICLES ALL OVER FIELD" << std::endl; const int x_gap = 10, y_gap = 10; + int index = 0; num_particles = field.width * field.length / (x_gap * y_gap); + particles.resize(num_particles); for (int i = 0; i <= field.length; i += x_gap) { for (int j = 0; j <= field.width; j += y_gap) { @@ -100,7 +103,7 @@ void Robot::init_particles(bool initial_localization) new_particle.orientation = orientation; new_particle.weight = 1.0 / num_particles; - particles.push_back(new_particle); + particles[index++] = new_particle; } } kidnap_counter = 0; @@ -112,6 +115,8 @@ void Robot::resample_particles() std::vector new_particles; std::random_device xrd, yrd, wrd; + // TODO: Refactor this resample method + particles.resize(num_particles); for (const auto & p : particles) { if (p.weight >= 1.0 / (particles.size() * 10.0)) { new_particles.push_back(p); @@ -214,8 +219,8 @@ double Robot::calculate_object_likelihood( dx = measurement.position.x * 100; dy = measurement.position.y * 100; - x_rot = dx * cos(particle.orientation.radian()) - dy * sin(particle.orientation.radian()); - y_rot = dx * sin(particle.orientation.radian()) + dy * cos(particle.orientation.radian()); + x_rot = dx * particle.orientation.cos() - dy * particle.orientation.sin(); + y_rot = dx * particle.orientation.sin() + dy * particle.orientation.cos(); relative_position_x = particle.position.x + x_rot; relative_position_y = particle.position.y + y_rot; From d7b1f5af490a2549a6b2141b5a16dbb6be3d6dc9 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sat, 1 Mar 2025 23:14:58 +0700 Subject: [PATCH 26/45] feat: enhance resampling and pose estimate method --- include/suiryoku/locomotion/model/robot.hpp | 16 +- src/suiryoku/locomotion/model/robot.cpp | 284 ++++++++++++------ .../locomotion/node/locomotion_node.cpp | 9 +- 3 files changed, 208 insertions(+), 101 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 27bd65c..ab710b9 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -21,6 +21,7 @@ #ifndef SUIRYOKU__LOCOMOTION__MODEL__ROBOT_HPP_ #define SUIRYOKU__LOCOMOTION__MODEL__ROBOT_HPP_ +#include #include #include "keisan/keisan.hpp" @@ -51,14 +52,15 @@ class Robot keisan::Angle get_tilt() const; // localizations - void localize(bool initial_localization = false); - void init_particles(bool initial_localization); + void localize(); + void init_particles(); void resample_particles(); void update_motion(); void calculate_weight(); void estimate_position(); void print_particles(); void print_estimate_position(); + void set_initial_localization(bool initial) { initial_localization = initial; } double calculate_total_likelihood(const Particle & particle); double calculate_object_likelihood(const ProjectedObject & measurement, const Particle & particle); double get_sum_weight(); @@ -101,6 +103,16 @@ class Robot double xvar; double yvar; int kidnap_counter; + Particle best_particle; + + std::mt19937 rand_gen; + double weight_avg; + double short_term_avg; + double long_term_avg; + double last_weight_avg; + + bool initial_localization; + bool reset_particles; }; } // namespace suiryoku diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 2f3a049..921a3c0 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -19,7 +19,6 @@ // THE SOFTWARE. #include -#include #include #include "suiryoku/locomotion/model/robot.hpp" @@ -36,7 +35,10 @@ Robot::Robot() y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), is_calibrated(false), use_localization(false), apply_localization(false), - num_particles(0), xvar(10.0), yvar(10.0), kidnap_counter(0) + num_particles(500), xvar(10.0), yvar(10.0), kidnap_counter(0), weight_avg(0.0), + rand_gen(std::random_device{}()), short_term_avg(0.0), long_term_avg(0.0), + last_weight_avg(0.0), estimated_position(0.0, 0.0), + initial_localization(true), reset_particles(false) { } @@ -50,91 +52,151 @@ keisan::Angle Robot::get_tilt() const return tilt + tilt_center; } -void Robot::localize(bool initial_localization) +void Robot::localize() { - if (num_particles == 0 || initial_localization) { - init_particles(initial_localization); + printf("initialize localization: %d\n", initial_localization); + if (initial_localization) { + printf("init particles start\n"); + init_particles(); + printf("init particles done\n"); } else { + printf("update motion start\n"); update_motion(); + printf("update motion done\n"); } - if (projected_objects.empty()) { - return; - } + if (!projected_objects.empty()) { + printf("calculate weight start\n"); + calculate_weight(); + printf("calculate weight done\n"); + if (std::isnan(weight_avg)) { + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + printf("weight_avg is nan\n"); + weight_avg = last_weight_avg; + } else { + if (initial_localization) { + short_term_avg = weight_avg; + long_term_avg = weight_avg; + initial_localization = false; + printf("INITIAL LOCALIZATION FALSE\n"); + printf("INITIAL LOCALIZATION FALSE\n"); + printf("INITIAL LOCALIZATION FALSE\n"); + printf("INITIAL LOCALIZATION FALSE\n"); + printf("INITIAL LOCALIZATION FALSE\n"); + printf("INITIAL LOCALIZATION FALSE\n"); + printf("INITIAL LOCALIZATION FALSE\n"); + printf("INITIAL LOCALIZATION FALSE\n"); + printf("INITIAL LOCALIZATION FALSE\n"); + printf("INITIAL LOCALIZATION FALSE\n"); + printf("INITIAL LOCALIZATION FALSE\n"); + } + last_weight_avg = weight_avg; + } - calculate_weight(); - resample_particles(); + short_term_avg = 0.1 * (weight_avg - short_term_avg); + long_term_avg = 0.001 * (weight_avg - long_term_avg); - estimate_position(); - print_estimate_position(); + printf("resample particles start\n"); + resample_particles(); + printf("resample particles done\n"); + + printf("estimate position start\n"); + estimate_position(); + printf("estimate position done\n"); + + // print_estimate_position(); + print_particles(); + } } -void Robot::init_particles(bool initial_localization) +void Robot::init_particles() { + double uniform_weight = 1.0 / num_particles; + num_particles = 500; particles.clear(); - if (initial_localization) { - std::cout << "INIT PARTICLES BASED ON LAST POSE" << std::endl; - - num_particles = 1000; - std::random_device xrd, yrd; - std::normal_distribution xrg(position.x, xvar), yrg(position.y, yvar); - - particles.resize(num_particles); - for (int i = 0; i < num_particles; ++i) { - Particle new_particle; - new_particle.position = keisan::Point2(xrg(xrd), yrg(yrd)); - new_particle.orientation = orientation; - new_particle.weight = 1.0 / num_particles; - - particles[i] = new_particle; - } - } else { // if not initial, generate particles all over the field - std::cout << "INIT PARTICLES ALL OVER FIELD" << std::endl; - - const int x_gap = 10, y_gap = 10; - int index = 0; - num_particles = field.width * field.length / (x_gap * y_gap); - particles.resize(num_particles); - - for (int i = 0; i <= field.length; i += x_gap) { - for (int j = 0; j <= field.width; j += y_gap) { - Particle new_particle; - new_particle.position = keisan::Point2(i, j); - new_particle.orientation = orientation; - new_particle.weight = 1.0 / num_particles; - - particles[index++] = new_particle; - } - } - kidnap_counter = 0; + particles.resize(num_particles); + // std::uniform_int_distribution xrg(position.x - 100, position.x + 100); + // std::uniform_int_distribution yrg(position.y - 100, position.y + 100); + std::uniform_int_distribution xrg(0, 900); + std::uniform_int_distribution yrg(0, 600); + + for (int i = 0; i < num_particles; ++i) { + particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + particles[i].orientation = orientation; + particles[i].weight = uniform_weight; } } void Robot::resample_particles() { - std::vector new_particles; - std::random_device xrd, yrd, wrd; + std::vector& old_particles = particles; + std::vector new_particles(num_particles); + Particle current_best_particle; + + std::uniform_int_distribution rand_index(0, num_particles - 1); + int index = rand_index(rand_gen); + + double beta = 0.0; + double max_weight = 0.0; + double prob = std::max(0.0, 1.0 - short_term_avg/long_term_avg); + reset_particles = prob > 0.25; + + // find the best particle + for (const auto & p : old_particles) { + if (p.weight > max_weight) { + max_weight = p.weight; + current_best_particle = p; + } + } - // TODO: Refactor this resample method - particles.resize(num_particles); - for (const auto & p : particles) { - if (p.weight >= 1.0 / (particles.size() * 10.0)) { - new_particles.push_back(p); - std::normal_distribution xrg(p.position.x, xvar), yrg(p.position.y, yvar); - - int n = p.weight * 100; - for (int i = 0; i < n; ++i) { - Particle new_particle; - new_particle.position = keisan::Point2(xrg(xrd), yrg(yrd)); - new_particle.orientation = orientation; - new_particle.weight = 1.0 / n; - - new_particles.push_back(new_particle); + best_particle = current_best_particle; + + // determine resample interval area + double interval_x[2] = {0.0, 900.0}; + double interval_y[2] = {0.0, 600.0}; + + // if (!projected_objects.empty()) { + interval_x[0] = position.x - 100; + interval_x[1] = position.x + 100; + interval_y[0] = position.y - 100; + interval_y[1] = position.y + 100; + // } + + // projected_objects.clear(); + + // resample particles + std::uniform_real_distribution rand_prob(0.0, 1.0); + for (int i = 0; i < num_particles; ++i) { + if (rand_prob(rand_gen) < prob) { + std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); + std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); + new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + new_particles[i].orientation = orientation; + new_particles[i].weight = 0.0; + } else { + std::uniform_real_distribution rand_beta(0.0, 2.0 * max_weight); + beta += rand_beta(rand_gen); + + while (beta > old_particles[index].weight) { + beta -= old_particles[index].weight; + index = (index + 1) % num_particles; } + + new_particles[i] = old_particles[index]; } } - particles = new_particles; - num_particles = particles.size(); } void Robot::update_motion() @@ -142,6 +204,7 @@ void Robot::update_motion() static std::random_device xrd, yrd, wrd; static std::normal_distribution<> xgen(0.0, xvar), ygen(0.0, yvar); + printf("start update motion iteration\n"); for (auto & p : particles) { double static_noise_x = xgen(xrd) / 5.0; double static_noise_y = ygen(yrd) / 5.0; @@ -153,6 +216,7 @@ void Robot::update_motion() p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; p.orientation = orientation; } + printf("end update motion iteration\n"); } double Robot::get_sum_weight() @@ -171,20 +235,9 @@ void Robot::calculate_weight() } double sum_weight = get_sum_weight(); - if (sum_weight > 0.0) { - for (auto & p : particles) { - p.weight /= sum_weight; - } - kidnap_counter = 0; - } else { - if (kidnap_counter++ < 1) { - init_particles(true); - } else { - init_particles(false); - } - } - - projected_objects.clear(); + weight_avg = sum_weight / num_particles; + printf("sum_weight: %f\n", sum_weight); + printf("weight_avg: %f\n", weight_avg); } double Robot::calculate_total_likelihood(const Particle & particle) { @@ -204,6 +257,8 @@ double Robot::calculate_object_likelihood( double relative_position_x, relative_position_y; double dx, dy, x_rot, y_rot, exponent, likelihood; double current_likelihood = 0.0; + // keisan::Angle fov = 78.0_deg; + // keisan::Angle camera_orientation = orientation - get_pan(); if (measurement.label == "L-Intersection") { landmarks = field.landmarks_L; @@ -216,6 +271,12 @@ double Robot::calculate_object_likelihood( } for (int i = 0; i < landmarks.size(); i++) { + // check if the landmark is in the camera field of view + // keisan::Angle angle_to_landmark = keisan::signed_arctan( + // landmarks[i].y - particle.position.y, landmarks[i].x - particle.position.x); + // keisan::Angle angle_diff = angle_to_landmark - camera_orientation; + + // if (std::fabs(angle_diff.degree()) <= fov.degree() / 2) { dx = measurement.position.x * 100; dy = measurement.position.y * 100; @@ -225,45 +286,80 @@ double Robot::calculate_object_likelihood( relative_position_x = particle.position.x + x_rot; relative_position_y = particle.position.y + y_rot; + printf("landmark_x: %f | landmark_y: %f\n", landmarks[i].x, landmarks[i].y); + printf("relative_position_x: %f | relative_position_y: %f\n", relative_position_x, relative_position_y); + exponent = -0.5 * (pow((landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + pow((landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y); + printf("likelihood: %f | exponent: %f\n", likelihood, exponent); + printf("sigma_x: %f | sigma_y: %f\n", sigma_x, sigma_y); + printf("exp(exponent): %f\n", exp(exponent)); if (likelihood > current_likelihood) { current_likelihood = likelihood; } + // } } return current_likelihood; } void Robot::estimate_position() { - if (num_particles == 0 || get_sum_weight() == 0) { - return; - } - - double x_mean = 0.0; - double y_mean = 0.0; - for (const auto & p : particles) { - x_mean += (1.0 / num_particles) * p.position.x; - y_mean += (1.0 / num_particles) * p.position.y; + keisan::Point2 sum_position = {0.0, 0.0}; + int centered_particles = 0; + + // Find numbers of particles closed to the best particle + for (int i = 0; i < num_particles; ++i) { + double distance = sqrt(pow(particles[i].position.x - best_particle.position.x, 2) + + pow(particles[i].position.y - best_particle.position.y, 2)); + + if (distance < 25.0) { + centered_particles++; + sum_position.x += particles[i].position.x; + sum_position.y += particles[i].position.y; + } } - estimated_position.x = (x_mean < 0.0) ? 0.0 : (x_mean > 900.0 ? 900.0 : x_mean); - estimated_position.y = (y_mean < 0.0) ? 0.0 : (y_mean > 600.0 ? 600.0 : y_mean); - if (num_particles < 1000) { + // Use mean of centered particles position if more than 30% of particles are centered + if (reset_particles || centered_particles < 0.3 * num_particles) { + if (reset_particles) { + printf("RESET PARTICLES\n"); + printf("RESET PARTICLES\n"); + printf("RESET PARTICLES\n"); + printf("RESET PARTICLES\n"); + } else if (centered_particles < 0.3 * num_particles) { + printf("CENTERED PARTICLES NOT SATISFIED\n"); + printf("CENTERED PARTICLES NOT SATISFIED\n"); + printf("CENTERED PARTICLES NOT SATISFIED\n"); + printf("CENTERED PARTICLES NOT SATISFIED\n"); + } + estimated_position = position; + } else { + printf("CENTERED PARTICLES SATISFIED\n"); + printf("CENTERED PARTICLES SATISFIED\n"); + printf("CENTERED PARTICLES SATISFIED\n"); + printf("CENTERED PARTICLES SATISFIED\n"); + printf("CENTERED PARTICLES SATISFIED\n"); + printf("CENTERED PARTICLES SATISFIED\n"); + printf("CENTERED PARTICLES SATISFIED\n"); + printf("CENTERED PARTICLES SATISFIED\n"); + estimated_position.x = sum_position.x / centered_particles; + estimated_position.y = sum_position.y / centered_particles; position = estimated_position; apply_localization = true; } + + reset_particles = false; } void Robot::print_particles() { double sum_samples = 0.0; - for (int i = 0; i < num_particles; i++) { + for (int i = 0; i < num_particles; ++i) { if (particles[i].weight > 0.0001) { std::cout << "Particle " << std::setw(5) << i << " weight: " << std::fixed << std::setprecision(5) diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index eb95a20..4d2f099 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -79,11 +79,6 @@ LocomotionNode::LocomotionNode( [this](const Point2::SharedPtr message) { this->robot->delta_position.x = message->x; this->robot->delta_position.y = message->y; - - if (this->robot->use_localization && - (message->x || message->y || this->robot->a_speed)) { - this->robot->localize(); - } }); projected_objects_subscriber = node->create_subscription( @@ -105,6 +100,10 @@ LocomotionNode::LocomotionNode( void LocomotionNode::update() { + // printf("Starting localization\n"); + this->robot->localize(); + // printf("Localization done\n"); + publish_walking(); if (set_odometry || this->robot->apply_localization) { this->robot->apply_localization = false; From 8f4fee12430ee5c7feb7ceaef4f48eff552386dd Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 2 Mar 2025 15:56:53 +0700 Subject: [PATCH 27/45] fix: apply localization and update position bug --- include/suiryoku/locomotion/model/robot.hpp | 4 +- src/suiryoku/locomotion/model/robot.cpp | 142 +++++++----------- .../locomotion/node/locomotion_node.cpp | 14 +- 3 files changed, 69 insertions(+), 91 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index ab710b9..c21e125 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -99,6 +99,9 @@ class Robot double a_speed; bool aim_on; + bool reset_particles; // for debug, change to private later + std::vector center_particles; // for debug, erase later + private: double xvar; double yvar; @@ -112,7 +115,6 @@ class Robot double last_weight_avg; bool initial_localization; - bool reset_particles; }; } // namespace suiryoku diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 921a3c0..e40e537 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -54,80 +54,41 @@ keisan::Angle Robot::get_tilt() const void Robot::localize() { - printf("initialize localization: %d\n", initial_localization); if (initial_localization) { - printf("init particles start\n"); init_particles(); - printf("init particles done\n"); } else { - printf("update motion start\n"); update_motion(); - printf("update motion done\n"); } if (!projected_objects.empty()) { printf("calculate weight start\n"); calculate_weight(); printf("calculate weight done\n"); - if (std::isnan(weight_avg)) { - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); - printf("weight_avg is nan\n"); + if (weight_avg <= 0 || std::isnan(weight_avg)) { weight_avg = last_weight_avg; } else { if (initial_localization) { short_term_avg = weight_avg; long_term_avg = weight_avg; initial_localization = false; - printf("INITIAL LOCALIZATION FALSE\n"); - printf("INITIAL LOCALIZATION FALSE\n"); - printf("INITIAL LOCALIZATION FALSE\n"); - printf("INITIAL LOCALIZATION FALSE\n"); - printf("INITIAL LOCALIZATION FALSE\n"); - printf("INITIAL LOCALIZATION FALSE\n"); - printf("INITIAL LOCALIZATION FALSE\n"); - printf("INITIAL LOCALIZATION FALSE\n"); - printf("INITIAL LOCALIZATION FALSE\n"); - printf("INITIAL LOCALIZATION FALSE\n"); - printf("INITIAL LOCALIZATION FALSE\n"); } last_weight_avg = weight_avg; - } - - short_term_avg = 0.1 * (weight_avg - short_term_avg); - long_term_avg = 0.001 * (weight_avg - long_term_avg); - printf("resample particles start\n"); + short_term_avg = 0.1 * (weight_avg - short_term_avg); + long_term_avg = 0.001 * (weight_avg - long_term_avg); + } resample_particles(); - printf("resample particles done\n"); - - printf("estimate position start\n"); estimate_position(); - printf("estimate position done\n"); - - // print_estimate_position(); - print_particles(); + projected_objects.clear(); } } void Robot::init_particles() { double uniform_weight = 1.0 / num_particles; - num_particles = 500; particles.clear(); particles.resize(num_particles); - // std::uniform_int_distribution xrg(position.x - 100, position.x + 100); + // std::uniform_int_distribution xrg(position.x - 100, position.x + 100); // use if initial position is known // std::uniform_int_distribution yrg(position.y - 100, position.y + 100); std::uniform_int_distribution xrg(0, 900); std::uniform_int_distribution yrg(0, 600); @@ -167,24 +128,23 @@ void Robot::resample_particles() double interval_x[2] = {0.0, 900.0}; double interval_y[2] = {0.0, 600.0}; - // if (!projected_objects.empty()) { + if (!projected_objects.empty()) { interval_x[0] = position.x - 100; interval_x[1] = position.x + 100; interval_y[0] = position.y - 100; interval_y[1] = position.y + 100; - // } - - // projected_objects.clear(); + } // resample particles std::uniform_real_distribution rand_prob(0.0, 1.0); + double uniform_weight = 1.0 / num_particles; for (int i = 0; i < num_particles; ++i) { if (rand_prob(rand_gen) < prob) { std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); new_particles[i].orientation = orientation; - new_particles[i].weight = 0.0; + new_particles[i].weight = uniform_weight; } else { std::uniform_real_distribution rand_beta(0.0, 2.0 * max_weight); beta += rand_beta(rand_gen); @@ -204,7 +164,6 @@ void Robot::update_motion() static std::random_device xrd, yrd, wrd; static std::normal_distribution<> xgen(0.0, xvar), ygen(0.0, yvar); - printf("start update motion iteration\n"); for (auto & p : particles) { double static_noise_x = xgen(xrd) / 5.0; double static_noise_y = ygen(yrd) / 5.0; @@ -216,7 +175,6 @@ void Robot::update_motion() p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; p.orientation = orientation; } - printf("end update motion iteration\n"); } double Robot::get_sum_weight() @@ -234,10 +192,13 @@ void Robot::calculate_weight() p.weight = calculate_total_likelihood(p); } + // normalize weights double sum_weight = get_sum_weight(); + for (auto & p : particles) { + p.weight /= sum_weight; + } + weight_avg = sum_weight / num_particles; - printf("sum_weight: %f\n", sum_weight); - printf("weight_avg: %f\n", weight_avg); } double Robot::calculate_total_likelihood(const Particle & particle) { @@ -286,18 +247,12 @@ double Robot::calculate_object_likelihood( relative_position_x = particle.position.x + x_rot; relative_position_y = particle.position.y + y_rot; - printf("landmark_x: %f | landmark_y: %f\n", landmarks[i].x, landmarks[i].y); - printf("relative_position_x: %f | relative_position_y: %f\n", relative_position_x, relative_position_y); - exponent = - -0.5 * - (pow((landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + - pow((landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); + -0.5 * + (pow((landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + + pow((landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); - likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y); - printf("likelihood: %f | exponent: %f\n", likelihood, exponent); - printf("sigma_x: %f | sigma_y: %f\n", sigma_x, sigma_y); - printf("exp(exponent): %f\n", exp(exponent)); + likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * 100000; if (likelihood > current_likelihood) { current_likelihood = likelihood; @@ -311,49 +266,35 @@ double Robot::calculate_object_likelihood( void Robot::estimate_position() { keisan::Point2 sum_position = {0.0, 0.0}; int centered_particles = 0; + center_particles.clear(); // Find numbers of particles closed to the best particle for (int i = 0; i < num_particles; ++i) { double distance = sqrt(pow(particles[i].position.x - best_particle.position.x, 2) + pow(particles[i].position.y - best_particle.position.y, 2)); - if (distance < 25.0) { + if (distance < 50.0) { centered_particles++; sum_position.x += particles[i].position.x; sum_position.y += particles[i].position.y; + + center_particles.push_back(particles[i]); } } // Use mean of centered particles position if more than 30% of particles are centered - if (reset_particles || centered_particles < 0.3 * num_particles) { - if (reset_particles) { - printf("RESET PARTICLES\n"); - printf("RESET PARTICLES\n"); - printf("RESET PARTICLES\n"); - printf("RESET PARTICLES\n"); - } else if (centered_particles < 0.3 * num_particles) { - printf("CENTERED PARTICLES NOT SATISFIED\n"); - printf("CENTERED PARTICLES NOT SATISFIED\n"); - printf("CENTERED PARTICLES NOT SATISFIED\n"); - printf("CENTERED PARTICLES NOT SATISFIED\n"); - } - estimated_position = position; - } else { - printf("CENTERED PARTICLES SATISFIED\n"); - printf("CENTERED PARTICLES SATISFIED\n"); - printf("CENTERED PARTICLES SATISFIED\n"); - printf("CENTERED PARTICLES SATISFIED\n"); - printf("CENTERED PARTICLES SATISFIED\n"); - printf("CENTERED PARTICLES SATISFIED\n"); - printf("CENTERED PARTICLES SATISFIED\n"); - printf("CENTERED PARTICLES SATISFIED\n"); + if (reset_particles || centered_particles < 3) { + estimated_position = position + delta_position; + } else{ estimated_position.x = sum_position.x / centered_particles; estimated_position.y = sum_position.y / centered_particles; + + estimated_position.x = std::max(0.0, std::min(900.0, estimated_position.x)); + estimated_position.y = std::max(0.0, std::min(600.0, estimated_position.y)); + position = estimated_position; apply_localization = true; } - - reset_particles = false; } void Robot::print_particles() { @@ -374,8 +315,31 @@ void Robot::print_particles() { } } + printf("========================================\n"); + + printf("Centered particles: %d\n", center_particles.size()); + for (int i = 0; i < center_particles.size(); ++i) { + std::cout << "Centered Particle " << std::setw(5) << i + << " weight: " << std::fixed << std::setprecision(5) + << center_particles[i].weight << std::setw(5) << " [" + << std::fixed << std::setprecision(2) << center_particles[i].position.x + << ", " << std::fixed << std::setprecision(2) + << center_particles[i].position.y << ", " << std::fixed + << std::setprecision(2) << center_particles[i].orientation.degree() << "]" + << std::endl; + } + + std::cout << "Best particle: " << std::fixed << std::setprecision(5) + << best_particle.weight << std::setw(5) << " [" + << std::fixed << std::setprecision(2) << best_particle.position.x + << ", " << std::fixed << std::setprecision(2) + << best_particle.position.y << ", " << std::fixed + << std::setprecision(2) << best_particle.orientation.degree() << "]" + << std::endl; + std::cout << "Num particles: " << num_particles << std::endl; std::cout << "Sum weights: " << sum_samples << std::endl; + std::cout << "Projected objects: " << projected_objects.size() << std::endl; print_estimate_position(); } diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 4d2f099..428ae4f 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -79,6 +79,15 @@ LocomotionNode::LocomotionNode( [this](const Point2::SharedPtr message) { this->robot->delta_position.x = message->x; this->robot->delta_position.y = message->y; + + bool run_localization = false; + run_localization |= message->x != 0.0; + run_localization |= message->y != 0.0; + run_localization |= this->robot->a_speed != 0.0; + + if (run_localization) { + this->robot->localize(); + } }); projected_objects_subscriber = node->create_subscription( @@ -101,11 +110,14 @@ LocomotionNode::LocomotionNode( void LocomotionNode::update() { // printf("Starting localization\n"); - this->robot->localize(); + // this->robot->localize(); // printf("Localization done\n"); publish_walking(); if (set_odometry || this->robot->apply_localization) { + if (this->robot->apply_localization) { + printf("Localization applied\n"); + } this->robot->apply_localization = false; publish_odometry(); } From 025cd469d6881f3d3d614fa1d1f29445d69a8815 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 2 Mar 2025 16:17:18 +0700 Subject: [PATCH 28/45] feat: set particles number use config --- include/suiryoku/locomotion/process/locomotion.hpp | 5 +++-- src/suiryoku/locomotion/process/locomotion.cpp | 2 ++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index cde91b1..125c71d 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -67,8 +67,8 @@ class Locomotion const keisan::Angle & direction); bool position_left_kick(const keisan::Angle & direction); bool position_right_kick(const keisan::Angle & direction); - bool position_kick_custom_pan_tilt(const keisan::Angle & direction, const keisan::Angle & min_pan, - const keisan::Angle & max_pan, const keisan::Angle & min_tilt, + bool position_kick_custom_pan_tilt(const keisan::Angle & direction, const keisan::Angle & min_pan, + const keisan::Angle & max_pan, const keisan::Angle & min_tilt, const keisan::Angle & max_tilt); bool position_kick_general(const keisan::Angle & direction); bool position_kick_range_pan_tilt(const keisan::Angle & direction, bool precise_kick, bool left_kick, bool is_positioning_center); @@ -168,6 +168,7 @@ class Locomotion keisan::Angle right_kick_target_tilt; bool localization_enable; + int num_particles; std::shared_ptr robot; }; diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index eca33d8..6caeed5 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -292,7 +292,9 @@ void Locomotion::set_config(const nlohmann::json & json) bool valid_section = true; valid_section &= jitsuyo::assign_val(localization_section, "enable", localization_enable); + valid_section &= jitsuyo::assign_val(localization_section, "num_particles", num_particles); robot->use_localization = localization_enable; + robot->num_particles = num_particles; if (!valid_section) { std::cout << "Error found at section `localization`" << std::endl; From d24ae4d67e684fd844a0694d18b20b2173498753 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 16 Mar 2025 00:53:43 +0700 Subject: [PATCH 29/45] fix: adjust constant values and resolve stuck particles --- include/suiryoku/locomotion/model/robot.hpp | 14 +- .../locomotion/process/locomotion.hpp | 3 - src/suiryoku/locomotion/model/robot.cpp | 176 +++++++++++------- .../locomotion/node/locomotion_node.cpp | 7 + .../locomotion/process/locomotion.cpp | 7 +- 5 files changed, 135 insertions(+), 72 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index c21e125..c7b738e 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -43,6 +43,12 @@ struct Particle double weight; }; +enum ResampleInterval +{ + CENTER, + FIELD +}; + class Robot { public: @@ -69,6 +75,7 @@ class Robot std::vector particles; keisan::Point2 estimated_position; int num_particles; + double min_centered_particles_ratio; bool use_localization; bool apply_localization; @@ -100,7 +107,7 @@ class Robot bool aim_on; bool reset_particles; // for debug, change to private later - std::vector center_particles; // for debug, erase later + std::vector center_particles; // for debug, erase later private: double xvar; @@ -115,6 +122,11 @@ class Robot double last_weight_avg; bool initial_localization; + + int current_resample_interval; + int too_low_particles_count; + + double prob; // for debug, remove later }; } // namespace suiryoku diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index 125c71d..49b55a9 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -167,9 +167,6 @@ class Locomotion keisan::Angle right_kick_target_pan; keisan::Angle right_kick_target_tilt; - bool localization_enable; - int num_particles; - std::shared_ptr robot; }; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index e40e537..216ef22 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -37,8 +37,8 @@ Robot::Robot() is_calibrated(false), use_localization(false), apply_localization(false), num_particles(500), xvar(10.0), yvar(10.0), kidnap_counter(0), weight_avg(0.0), rand_gen(std::random_device{}()), short_term_avg(0.0), long_term_avg(0.0), - last_weight_avg(0.0), estimated_position(0.0, 0.0), - initial_localization(true), reset_particles(false) + last_weight_avg(0.0), estimated_position(0.0, 0.0), initial_localization(true), + reset_particles(false), too_low_particles_count(0) { } @@ -60,10 +60,9 @@ void Robot::localize() update_motion(); } - if (!projected_objects.empty()) { - printf("calculate weight start\n"); + if (!projected_objects.empty()){ + weight_avg = 0.0; calculate_weight(); - printf("calculate weight done\n"); if (weight_avg <= 0 || std::isnan(weight_avg)) { weight_avg = last_weight_avg; } else { @@ -73,14 +72,15 @@ void Robot::localize() initial_localization = false; } last_weight_avg = weight_avg; - - short_term_avg = 0.1 * (weight_avg - short_term_avg); - long_term_avg = 0.001 * (weight_avg - long_term_avg); } + + short_term_avg = 0.2 * (weight_avg - short_term_avg); + long_term_avg = 0.005 * (weight_avg - long_term_avg); resample_particles(); - estimate_position(); - projected_objects.clear(); } + + estimate_position(); + projected_objects.clear(); } void Robot::init_particles() @@ -92,6 +92,7 @@ void Robot::init_particles() // std::uniform_int_distribution yrg(position.y - 100, position.y + 100); std::uniform_int_distribution xrg(0, 900); std::uniform_int_distribution yrg(0, 600); + current_resample_interval = ResampleInterval::FIELD; for (int i = 0; i < num_particles; ++i) { particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); @@ -105,34 +106,38 @@ void Robot::resample_particles() std::vector& old_particles = particles; std::vector new_particles(num_particles); Particle current_best_particle; - std::uniform_int_distribution rand_index(0, num_particles - 1); int index = rand_index(rand_gen); double beta = 0.0; double max_weight = 0.0; - double prob = std::max(0.0, 1.0 - short_term_avg/long_term_avg); - reset_particles = prob > 0.25; + double sum_weight = get_sum_weight(); + prob = std::max(0.0, 1.0 - short_term_avg/long_term_avg); + reset_particles = prob > 0.3; // find the best particle - for (const auto & p : old_particles) { - if (p.weight > max_weight) { - max_weight = p.weight; - current_best_particle = p; + if (sum_weight > 0) { + for (auto & p : old_particles) { + if (p.weight > max_weight) { + max_weight = p.weight; + current_best_particle = p; + } } + best_particle = current_best_particle; } - best_particle = current_best_particle; - // determine resample interval area double interval_x[2] = {0.0, 900.0}; double interval_y[2] = {0.0, 600.0}; - if (!projected_objects.empty()) { - interval_x[0] = position.x - 100; - interval_x[1] = position.x + 100; - interval_y[0] = position.y - 100; - interval_y[1] = position.y + 100; + if (!projected_objects.empty() && !reset_particles && sum_weight > 0) { + interval_x[0] = position.x - 75; + interval_x[1] = position.x + 75; + interval_y[0] = position.y - 75; + interval_y[1] = position.y + 75; + current_resample_interval = ResampleInterval::CENTER; + } else { + current_resample_interval = ResampleInterval::FIELD; } // resample particles @@ -153,27 +158,42 @@ void Robot::resample_particles() beta -= old_particles[index].weight; index = (index + 1) % num_particles; } - new_particles[i] = old_particles[index]; } } + + reset_particles = false; } void Robot::update_motion() { - static std::random_device xrd, yrd, wrd; static std::normal_distribution<> xgen(0.0, xvar), ygen(0.0, yvar); - for (auto & p : particles) { - double static_noise_x = xgen(xrd) / 5.0; - double static_noise_y = ygen(yrd) / 5.0; - double dynamic_noise_x = fabs(delta_position.x) * xgen(xrd) / 5.0; - double dynamic_noise_y = fabs(delta_position.y) * ygen(yrd) / 5.0; - double x_yterm = fabs(delta_position.y)*xgen(xrd) / 30.0; - double y_xterm = fabs(delta_position.x)*ygen(yrd) / 30.0; - p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; - p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; - p.orientation = orientation; + if (!projected_objects.empty()) { + for (auto & p : particles) { + double static_noise_x = xgen(rand_gen) / 5.0; + double static_noise_y = ygen(rand_gen) / 5.0; + double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; + double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; + double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; + double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0; + + p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; + p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; + p.orientation = orientation; + } + } else { + if (center_particles.empty()) { + best_particle.position = position + delta_position; + best_particle.orientation = orientation; + return; + } + + best_particle.position += delta_position; + for (auto & p : particles) { + p.position += delta_position; + p.orientation = orientation; + } } } @@ -194,8 +214,10 @@ void Robot::calculate_weight() // normalize weights double sum_weight = get_sum_weight(); - for (auto & p : particles) { - p.weight /= sum_weight; + if (sum_weight > 0 && !std::isnan(sum_weight)) { + for (auto & p : particles) { + p.weight /= sum_weight; + } } weight_avg = sum_weight / num_particles; @@ -219,7 +241,7 @@ double Robot::calculate_object_likelihood( double dx, dy, x_rot, y_rot, exponent, likelihood; double current_likelihood = 0.0; // keisan::Angle fov = 78.0_deg; - // keisan::Angle camera_orientation = orientation - get_pan(); + // keisan::Angle camera_orientation = orientation /* - get_pan() */; if (measurement.label == "L-Intersection") { landmarks = field.landmarks_L; @@ -228,16 +250,19 @@ double Robot::calculate_object_likelihood( } else if (measurement.label == "X-Intersection") { landmarks = field.landmarks_X; } else if (measurement.label == "goalpost") { - landmarks = field.landmarks_X; + landmarks = field.landmarks_goalpost; } for (int i = 0; i < landmarks.size(); i++) { // check if the landmark is in the camera field of view // keisan::Angle angle_to_landmark = keisan::signed_arctan( - // landmarks[i].y - particle.position.y, landmarks[i].x - particle.position.x); + // landmarks[i].y - particle.position.y, landmarks[i].x - particle.position.x); // keisan::Angle angle_diff = angle_to_landmark - camera_orientation; - // if (std::fabs(angle_diff.degree()) <= fov.degree() / 2) { + // if (std::fabs(angle_diff.degree()) > fov.degree() / 2) { + // continue; + // } + dx = measurement.position.x * 100; dy = measurement.position.y * 100; @@ -257,7 +282,6 @@ double Robot::calculate_object_likelihood( if (likelihood > current_likelihood) { current_likelihood = likelihood; } - // } } return current_likelihood; @@ -278,22 +302,27 @@ void Robot::estimate_position() { sum_position.x += particles[i].position.x; sum_position.y += particles[i].position.y; - center_particles.push_back(particles[i]); + center_particles.push_back(&particles[i]); } } // Use mean of centered particles position if more than 30% of particles are centered - if (reset_particles || centered_particles < 3) { - estimated_position = position + delta_position; - } else{ + if (centered_particles > min_centered_particles_ratio * num_particles) { estimated_position.x = sum_position.x / centered_particles; estimated_position.y = sum_position.y / centered_particles; - estimated_position.x = std::max(0.0, std::min(900.0, estimated_position.x)); - estimated_position.y = std::max(0.0, std::min(600.0, estimated_position.y)); - - position = estimated_position; - apply_localization = true; + if (estimated_position.x >= 0.0 && estimated_position.x <= 900.0 && + estimated_position.y >= 0.0 && estimated_position.y <= 600.0) { + position = estimated_position; + apply_localization = true; + } + too_low_particles_count = 0; + } else { + too_low_particles_count++; + if (too_low_particles_count > 50) { + reset_particles = true; + too_low_particles_count = 0; + } } } @@ -302,31 +331,50 @@ void Robot::print_particles() { for (int i = 0; i < num_particles; ++i) { if (particles[i].weight > 0.0001) { - std::cout << "Particle " << std::setw(5) << i - << " weight: " << std::fixed << std::setprecision(5) - << particles[i].weight << std::setw(5) << " [" - << std::fixed << std::setprecision(2) << particles[i].position.x - << ", " << std::fixed << std::setprecision(2) - << particles[i].position.y << ", " << std::fixed - << std::setprecision(2) << particles[i].orientation.degree() << "]" - << std::endl; + // std::cout << "Particle " << std::setw(5) << i + // << " weight: " << std::fixed << std::setprecision(5) + // << particles[i].weight << std::setw(5) << " [" + // << std::fixed << std::setprecision(2) << particles[i].position.x + // << ", " << std::fixed << std::setprecision(2) + // << particles[i].position.y << ", " << std::fixed + // << std::setprecision(2) << particles[i].orientation.degree() << "]" + // << std::endl; sum_samples += particles[i].weight; } } + std::string resample_interval; + switch (current_resample_interval) { + case ResampleInterval::CENTER: + resample_interval = "CENTER"; + break; + case ResampleInterval::FIELD: + resample_interval = "FIELD"; + break; + } + + printf("Resample interval: %s\n", resample_interval.c_str()); + printf("Prob: %.2f | is more than 0.1: %s\n", prob, (prob > 0.1) ? "true" : "false"); + printf("Reset particles: %s\n", reset_particles ? "true" : "false"); + printf("========================================\n"); + printf("Minimal Centered Particles: %.0f\n", min_centered_particles_ratio * num_particles); printf("Centered particles: %d\n", center_particles.size()); for (int i = 0; i < center_particles.size(); ++i) { std::cout << "Centered Particle " << std::setw(5) << i << " weight: " << std::fixed << std::setprecision(5) - << center_particles[i].weight << std::setw(5) << " [" - << std::fixed << std::setprecision(2) << center_particles[i].position.x + << center_particles[i]->weight << std::setw(5) << " [" + << std::fixed << std::setprecision(2) << center_particles[i]->position.x << ", " << std::fixed << std::setprecision(2) - << center_particles[i].position.y << ", " << std::fixed - << std::setprecision(2) << center_particles[i].orientation.degree() << "]" + << center_particles[i]->position.y << ", " << std::fixed + << std::setprecision(2) << center_particles[i]->orientation.degree() << "]" << std::endl; + if (i >= 10) { + printf("... and %d more\n", center_particles.size() - 10); + break; + } } std::cout << "Best particle: " << std::fixed << std::setprecision(5) diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 428ae4f..bcf5e2e 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -84,6 +84,7 @@ LocomotionNode::LocomotionNode( run_localization |= message->x != 0.0; run_localization |= message->y != 0.0; run_localization |= this->robot->a_speed != 0.0; + run_localization &= this->robot->use_localization; if (run_localization) { this->robot->localize(); @@ -95,6 +96,12 @@ LocomotionNode::LocomotionNode( [this](const ProjectedObjects::SharedPtr message) { this->robot->projected_objects.clear(); for (const auto & obj : message->projected_objects) { + if (obj.label == "ball" || obj.label == "robot" || obj.label == "self" || + obj.position.x < 0.0 || obj.position.x > 400.0 || + obj.position.y < 0.0 || obj.position.y > 300.0) { + continue; + } + this->robot->projected_objects.push_back( ProjectedObject{ obj.label, diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 6caeed5..a191845 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -291,10 +291,9 @@ void Locomotion::set_config(const nlohmann::json & json) if (jitsuyo::assign_val(json, "localization", localization_section)) { bool valid_section = true; - valid_section &= jitsuyo::assign_val(localization_section, "enable", localization_enable); - valid_section &= jitsuyo::assign_val(localization_section, "num_particles", num_particles); - robot->use_localization = localization_enable; - robot->num_particles = num_particles; + valid_section &= jitsuyo::assign_val(localization_section, "enable", robot->use_localization); + valid_section &= jitsuyo::assign_val(localization_section, "num_particles", robot->num_particles); + valid_section &= jitsuyo::assign_val(localization_section, "min_centered_particles_ratio", robot->min_centered_particles_ratio); if (!valid_section) { std::cout << "Error found at section `localization`" << std::endl; From 6298403d22ae2f592f42c8596841cef8d1e7444e Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 16 Mar 2025 17:00:20 +0700 Subject: [PATCH 30/45] fix: refine localization calculation --- include/suiryoku/locomotion/model/robot.hpp | 10 ++- src/suiryoku/locomotion/model/robot.cpp | 62 +++++++++++++++---- .../locomotion/node/locomotion_node.cpp | 29 +++++---- .../locomotion/process/locomotion.cpp | 31 +++++----- 4 files changed, 93 insertions(+), 39 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index c7b738e..03e602a 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -46,7 +46,10 @@ struct Particle enum ResampleInterval { CENTER, - FIELD + FIELD_INIT, + FIELD_RESET, + FIELD_EMPTY_PROJECTED_OBJECTS, + FIELD_ZERO_WEIGHT }; class Robot @@ -59,6 +62,7 @@ class Robot // localizations void localize(); + void reset_localization(); void init_particles(); void resample_particles(); void update_motion(); @@ -76,12 +80,16 @@ class Robot keisan::Point2 estimated_position; int num_particles; double min_centered_particles_ratio; + double short_term_avg_ratio; + double long_term_avg_ratio; + double reset_particles_threshold; bool use_localization; bool apply_localization; // IPM std::vector projected_objects; + int num_projected_objects; // member for getting bool is_calibrated; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 216ef22..925b6cb 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -38,7 +38,7 @@ Robot::Robot() num_particles(500), xvar(10.0), yvar(10.0), kidnap_counter(0), weight_avg(0.0), rand_gen(std::random_device{}()), short_term_avg(0.0), long_term_avg(0.0), last_weight_avg(0.0), estimated_position(0.0, 0.0), initial_localization(true), - reset_particles(false), too_low_particles_count(0) + reset_particles(false), too_low_particles_count(0), num_projected_objects(0) { } @@ -74,8 +74,8 @@ void Robot::localize() last_weight_avg = weight_avg; } - short_term_avg = 0.2 * (weight_avg - short_term_avg); - long_term_avg = 0.005 * (weight_avg - long_term_avg); + short_term_avg += short_term_avg_ratio * (weight_avg - short_term_avg); + long_term_avg += long_term_avg_ratio * (weight_avg - long_term_avg); resample_particles(); } @@ -83,6 +83,24 @@ void Robot::localize() projected_objects.clear(); } +void Robot::reset_localization() { + apply_localization = false; + initial_localization = true; + particles.clear(); + center_particles.clear(); + projected_objects.clear(); + estimated_position = keisan::Point2(0.0, 0.0); + best_particle = Particle(); + weight_avg = 0.0; + short_term_avg = 0.0; + long_term_avg = 0.0; + last_weight_avg = 0.0; + current_resample_interval = ResampleInterval::FIELD_INIT; + too_low_particles_count = 0; + reset_particles = false; + prob = 0.0; +} + void Robot::init_particles() { double uniform_weight = 1.0 / num_particles; @@ -92,7 +110,7 @@ void Robot::init_particles() // std::uniform_int_distribution yrg(position.y - 100, position.y + 100); std::uniform_int_distribution xrg(0, 900); std::uniform_int_distribution yrg(0, 600); - current_resample_interval = ResampleInterval::FIELD; + current_resample_interval = ResampleInterval::FIELD_INIT; for (int i = 0; i < num_particles; ++i) { particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); @@ -112,8 +130,8 @@ void Robot::resample_particles() double beta = 0.0; double max_weight = 0.0; double sum_weight = get_sum_weight(); - prob = std::max(0.0, 1.0 - short_term_avg/long_term_avg); - reset_particles = prob > 0.3; + prob = std::min(1.0, std::max(0.0, 1 - short_term_avg/long_term_avg)); + reset_particles = prob > reset_particles_threshold; // find the best particle if (sum_weight > 0) { @@ -137,7 +155,13 @@ void Robot::resample_particles() interval_y[1] = position.y + 75; current_resample_interval = ResampleInterval::CENTER; } else { - current_resample_interval = ResampleInterval::FIELD; + if (projected_objects.empty()) { + current_resample_interval = ResampleInterval::FIELD_EMPTY_PROJECTED_OBJECTS; + } else if (reset_particles) { + current_resample_interval = ResampleInterval::FIELD_RESET; + } else if (sum_weight <= 0) { + current_resample_interval = ResampleInterval::FIELD_ZERO_WEIGHT; + } } // resample particles @@ -236,7 +260,7 @@ double Robot::calculate_total_likelihood(const Particle & particle) { double Robot::calculate_object_likelihood( const ProjectedObject & measurement, const Particle & particle) { std::vector landmarks; - double sigma_x = 1.0, sigma_y = 1.0; + double sigma_x = 10.0, sigma_y = 10.0; double relative_position_x, relative_position_y; double dx, dy, x_rot, y_rot, exponent, likelihood; double current_likelihood = 0.0; @@ -264,7 +288,7 @@ double Robot::calculate_object_likelihood( // } dx = measurement.position.x * 100; - dy = measurement.position.y * 100; + dy = -measurement.position.y * 100; x_rot = dx * particle.orientation.cos() - dy * particle.orientation.sin(); y_rot = dx * particle.orientation.sin() + dy * particle.orientation.cos(); @@ -277,7 +301,7 @@ double Robot::calculate_object_likelihood( (pow((landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + pow((landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); - likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * 100000; + likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * 100; if (likelihood > current_likelihood) { current_likelihood = likelihood; @@ -349,8 +373,20 @@ void Robot::print_particles() { case ResampleInterval::CENTER: resample_interval = "CENTER"; break; - case ResampleInterval::FIELD: - resample_interval = "FIELD"; + case ResampleInterval::FIELD_EMPTY_PROJECTED_OBJECTS: + resample_interval = "FIELD_EMPTY_PROJECTED_OBJECTS"; + break; + case ResampleInterval::FIELD_INIT: + resample_interval = "FIELD_INIT"; + break; + case ResampleInterval::FIELD_RESET: + resample_interval = "FIELD_RESET"; + break; + case ResampleInterval::FIELD_ZERO_WEIGHT: + resample_interval = "FIELD_ZERO_WEIGHT"; + break; + default: + resample_interval = "UNKNOWN"; break; } @@ -387,7 +423,7 @@ void Robot::print_particles() { std::cout << "Num particles: " << num_particles << std::endl; std::cout << "Sum weights: " << sum_samples << std::endl; - std::cout << "Projected objects: " << projected_objects.size() << std::endl; + std::cout << "Projected objects: " << num_projected_objects << std::endl; print_estimate_position(); } diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index bcf5e2e..6e42613 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -80,15 +80,15 @@ LocomotionNode::LocomotionNode( this->robot->delta_position.x = message->x; this->robot->delta_position.y = message->y; - bool run_localization = false; - run_localization |= message->x != 0.0; - run_localization |= message->y != 0.0; - run_localization |= this->robot->a_speed != 0.0; - run_localization &= this->robot->use_localization; - - if (run_localization) { - this->robot->localize(); - } + // bool run_localization = false; + // run_localization |= message->x != 0.0; + // run_localization |= message->y != 0.0; + // run_localization |= this->robot->a_speed != 0.0; + // run_localization &= this->robot->use_localization; + + // if (run_localization) { + // this->robot->localize(); + // } }); projected_objects_subscriber = node->create_subscription( @@ -97,8 +97,8 @@ LocomotionNode::LocomotionNode( this->robot->projected_objects.clear(); for (const auto & obj : message->projected_objects) { if (obj.label == "ball" || obj.label == "robot" || obj.label == "self" || - obj.position.x < 0.0 || obj.position.x > 400.0 || - obj.position.y < 0.0 || obj.position.y > 300.0) { + obj.position.x < 0.0 || obj.position.x > 7.5 || + obj.position.y < -5.0 || obj.position.y > 5.0) { continue; } @@ -108,6 +108,13 @@ LocomotionNode::LocomotionNode( keisan::Point3{obj.position.x, obj.position.y, obj.position.z} }); } + + this->robot->num_projected_objects = this->robot->projected_objects.size(); + + if (!this->robot->projected_objects.empty() && this->robot->use_localization) { + printf("localize\n"); + this->robot->localize(); + } }); locomotion->stop = [this]() {this->walking_state = false;}; diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index a191845..66b4a03 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -73,7 +73,7 @@ void Locomotion::set_config(const nlohmann::json & json) } else { valid_config = false; } - + nlohmann::json rotate_section; if (jitsuyo::assign_val(json, "rotate", rotate_section)) { bool valid_section = true; @@ -86,7 +86,7 @@ void Locomotion::set_config(const nlohmann::json & json) } else { valid_config = false; } - + nlohmann::json backward_section; if (jitsuyo::assign_val(json, "backward", backward_section)) { bool valid_section = true; @@ -100,7 +100,7 @@ void Locomotion::set_config(const nlohmann::json & json) } else { valid_config = false; } - + nlohmann::json dribble_section; if (jitsuyo::assign_val(json, "dribble", dribble_section)) { bool valid_section = true; @@ -119,7 +119,7 @@ void Locomotion::set_config(const nlohmann::json & json) } else { valid_config = false; } - + nlohmann::json follow_section; if (jitsuyo::assign_val(json, "follow", follow_section)) { bool valid_section = true; @@ -148,7 +148,7 @@ void Locomotion::set_config(const nlohmann::json & json) } else { valid_config = false; } - + nlohmann::json skew_section; if (jitsuyo::assign_val(json, "skew", skew_section)) { bool valid_section = true; @@ -164,7 +164,7 @@ void Locomotion::set_config(const nlohmann::json & json) } else { valid_config = false; } - + nlohmann::json pivot_section; if (jitsuyo::assign_val(json, "pivot", pivot_section)) { bool valid_section = true; @@ -181,7 +181,7 @@ void Locomotion::set_config(const nlohmann::json & json) valid_section &= jitsuyo::assign_val(pivot_section, "pan_range_a_speed", pivot_pan_range_a_speed); valid_section &= jitsuyo::assign_val(pivot_section, "target_tilt", pivot_target_tilt_double); valid_section &= jitsuyo::assign_val(pivot_section, "pivot_stop_limit", pivot_stop_limit_double); - + pivot_target_tilt = keisan::make_degree(pivot_target_tilt_double); pivot_stop_limit = keisan::make_degree(pivot_stop_limit_double); @@ -192,7 +192,7 @@ void Locomotion::set_config(const nlohmann::json & json) } else { valid_config = false; } - + nlohmann::json position_section; if (jitsuyo::assign_val(json, "position", position_section)) { bool valid_section = true; @@ -265,7 +265,7 @@ void Locomotion::set_config(const nlohmann::json & json) } else { valid_config = false; } - + nlohmann::json right_kick_section; if (jitsuyo::assign_val(json, "right_kick", right_kick_section)) { bool valid_section = true; @@ -294,6 +294,9 @@ void Locomotion::set_config(const nlohmann::json & json) valid_section &= jitsuyo::assign_val(localization_section, "enable", robot->use_localization); valid_section &= jitsuyo::assign_val(localization_section, "num_particles", robot->num_particles); valid_section &= jitsuyo::assign_val(localization_section, "min_centered_particles_ratio", robot->min_centered_particles_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", robot->short_term_avg_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "long_term_avg_ratio", robot->long_term_avg_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "reset_particles_threshold", robot->reset_particles_threshold); if (!valid_section) { std::cout << "Error found at section `localization`" << std::endl; @@ -431,7 +434,7 @@ bool Locomotion::move_forward_to(const keisan::Point2 & target) if (target_distance < 8.0) { return true; } - + auto direction = keisan::signed_arctan(delta_y, delta_x).normalize(); double delta_direction = (direction - robot->orientation).normalize().degree(); @@ -707,7 +710,7 @@ bool Locomotion::pivot(const keisan::Angle & direction) if (fabs(pan) > pivot_pan_range_a_speed) { a_speed = keisan::map(pan, -pivot_pan_range_a_speed, pivot_pan_range_a_speed, pivot_max_a, -pivot_max_a); } - + robot->x_speed = x_speed; robot->y_speed = y_speed; robot->a_speed = a_speed; @@ -848,8 +851,8 @@ bool Locomotion::position_kick_general(const keisan::Angle & direction) (left_kick_target_tilt > right_kick_target_tilt ? left_kick_target_tilt : right_kick_target_tilt) + position_min_delta_tilt); } -bool Locomotion::position_kick_custom_pan_tilt(const keisan::Angle & direction, const keisan::Angle & min_pan, - const keisan::Angle & max_pan, const keisan::Angle & min_tilt, const keisan::Angle & max_tilt) +bool Locomotion::position_kick_custom_pan_tilt(const keisan::Angle & direction, const keisan::Angle & min_pan, + const keisan::Angle & max_pan, const keisan::Angle & min_tilt, const keisan::Angle & max_tilt) { double pan = (robot->get_pan() + robot->pan_center).degree(); double tilt = (robot->get_tilt() + robot->tilt_center).degree(); @@ -932,7 +935,7 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle & dire double delta_pan = (target_pan - pan).degree(); double y_speed = 0.0; - + if (!pan_in_range) { if (delta_pan < -position_min_delta_pan.degree()) { y_speed = keisan::map(delta_pan, -20.0, -position_min_delta_pan.degree(), position_max_ly, position_min_ly); From 126190de3c8ae484aaff9e07fea7b7ab794abc23 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Wed, 19 Mar 2025 23:18:17 +0700 Subject: [PATCH 31/45] feat: add sigma to config --- include/suiryoku/locomotion/model/robot.hpp | 2 ++ src/suiryoku/locomotion/model/robot.cpp | 10 +++++----- src/suiryoku/locomotion/process/locomotion.cpp | 2 ++ 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 03e602a..99a5483 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -83,6 +83,8 @@ class Robot double short_term_avg_ratio; double long_term_avg_ratio; double reset_particles_threshold; + double sigma_x; + double sigma_y; bool use_localization; bool apply_localization; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 925b6cb..be02e01 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -38,7 +38,8 @@ Robot::Robot() num_particles(500), xvar(10.0), yvar(10.0), kidnap_counter(0), weight_avg(0.0), rand_gen(std::random_device{}()), short_term_avg(0.0), long_term_avg(0.0), last_weight_avg(0.0), estimated_position(0.0, 0.0), initial_localization(true), - reset_particles(false), too_low_particles_count(0), num_projected_objects(0) + reset_particles(false), too_low_particles_count(0), num_projected_objects(0), + sigma_x(1.0), sigma_y(1.0), short_term_avg_ratio(0.1), long_term_avg_ratio(0.001) { } @@ -260,7 +261,6 @@ double Robot::calculate_total_likelihood(const Particle & particle) { double Robot::calculate_object_likelihood( const ProjectedObject & measurement, const Particle & particle) { std::vector landmarks; - double sigma_x = 10.0, sigma_y = 10.0; double relative_position_x, relative_position_y; double dx, dy, x_rot, y_rot, exponent, likelihood; double current_likelihood = 0.0; @@ -288,10 +288,10 @@ double Robot::calculate_object_likelihood( // } dx = measurement.position.x * 100; - dy = -measurement.position.y * 100; + dy = measurement.position.y * 100; - x_rot = dx * particle.orientation.cos() - dy * particle.orientation.sin(); - y_rot = dx * particle.orientation.sin() + dy * particle.orientation.cos(); + x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); + y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); relative_position_x = particle.position.x + x_rot; relative_position_y = particle.position.y + y_rot; diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 66b4a03..8ac0569 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -297,6 +297,8 @@ void Locomotion::set_config(const nlohmann::json & json) valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", robot->short_term_avg_ratio); valid_section &= jitsuyo::assign_val(localization_section, "long_term_avg_ratio", robot->long_term_avg_ratio); valid_section &= jitsuyo::assign_val(localization_section, "reset_particles_threshold", robot->reset_particles_threshold); + valid_section &= jitsuyo::assign_val(localization_section, "sigma_x", robot->sigma_x); + valid_section &= jitsuyo::assign_val(localization_section, "sigma_y", robot->sigma_y); if (!valid_section) { std::cout << "Error found at section `localization`" << std::endl; From 2f35942cc745b54ea8999ade2acf27e927ce16bc Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Thu, 20 Mar 2025 01:33:32 +0700 Subject: [PATCH 32/45] feat: add particles publisher --- .../locomotion/node/locomotion_node.hpp | 10 ++++++++++ .../locomotion/node/locomotion_node.cpp | 20 +++++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/include/suiryoku/locomotion/node/locomotion_node.hpp b/include/suiryoku/locomotion/node/locomotion_node.hpp index 7ad954a..6a9ce67 100644 --- a/include/suiryoku/locomotion/node/locomotion_node.hpp +++ b/include/suiryoku/locomotion/node/locomotion_node.hpp @@ -33,6 +33,9 @@ #include "rclcpp/rclcpp.hpp" #include "suiryoku/locomotion/model/robot.hpp" #include "suiryoku/locomotion/process/locomotion.hpp" +#include "suiryoku_interfaces/msg/particle.hpp" +#include "suiryoku_interfaces/msg/particles.hpp" + namespace suiryoku { @@ -42,6 +45,8 @@ class LocomotionNode public: using Head = atama_interfaces::msg::Head; using MeasurementStatus = kansei_interfaces::msg::Status; + using Particle = suiryoku_interfaces::msg::Particle; + using Particles = suiryoku_interfaces::msg::Particles; using Point2 = aruku_interfaces::msg::Point2; using ProjectedObjects = gyakuenki_interfaces::msg::ProjectedObjects; using SetWalking = aruku_interfaces::msg::SetWalking; @@ -59,14 +64,19 @@ class LocomotionNode private: void publish_walking(); void publish_odometry(); + void publish_particles(); rclcpp::Node::SharedPtr node; rclcpp::Publisher::SharedPtr set_walking_publisher; rclcpp::Publisher::SharedPtr set_odometry_publisher; + + rclcpp::Publisher::SharedPtr particles_publisher; + rclcpp::Subscription::SharedPtr measurement_status_subscriber; + rclcpp::Subscription::SharedPtr walking_status_subscriber; rclcpp::Subscription::SharedPtr head_subscriber; diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 6e42613..f8c3781 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -55,6 +55,9 @@ LocomotionNode::LocomotionNode( set_odometry_publisher = node->create_publisher( aruku::WalkingNode::set_odometry_topic(), 10); + particles_publisher = node->create_publisher( + "/localization/particles", 10); + walking_status_subscriber = node->create_subscription( aruku::WalkingNode::status_topic(), 10, [this](const WalkingStatus::SharedPtr message) @@ -114,6 +117,7 @@ LocomotionNode::LocomotionNode( if (!this->robot->projected_objects.empty() && this->robot->use_localization) { printf("localize\n"); this->robot->localize(); + publish_particles(); } }); @@ -161,4 +165,20 @@ void LocomotionNode::publish_odometry() set_odometry = false; } +void LocomotionNode::publish_particles() +{ + Particles particles_msg; + for (const auto & p : robot->particles) { + Particle particle_msg; + particle_msg.x = p.position.x; + particle_msg.y = p.position.y; + particle_msg.orientation = p.orientation.degree(); + particle_msg.weight = p.weight; + + particles_msg.particles.push_back(particle_msg); + } + + particles_publisher->publish(particles_msg); +} + } // namespace suiryoku From 7418faadad6230da2546d2b21705466f484d7fd0 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sat, 22 Mar 2025 01:54:43 +0700 Subject: [PATCH 33/45] fix: fix reset particles --- src/suiryoku/locomotion/model/robot.cpp | 44 +++++++++++++++---- .../locomotion/node/locomotion_node.cpp | 6 +-- 2 files changed, 38 insertions(+), 12 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index be02e01..7e0297e 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -55,6 +55,10 @@ keisan::Angle Robot::get_tilt() const void Robot::localize() { + if (!use_localization) { + return; + } + if (initial_localization) { init_particles(); } else { @@ -100,6 +104,10 @@ void Robot::reset_localization() { too_low_particles_count = 0; reset_particles = false; prob = 0.0; + + if (use_localization) { + localize(); + } } void Robot::init_particles() @@ -131,7 +139,8 @@ void Robot::resample_particles() double beta = 0.0; double max_weight = 0.0; double sum_weight = get_sum_weight(); - prob = std::min(1.0, std::max(0.0, 1 - short_term_avg/long_term_avg)); + // if (long_term_avg < 1e-6) long_term_avg = 1e-6; + prob = std::min(1.0, std::max(0.0, 1.0 - short_term_avg / long_term_avg)); reset_particles = prob > reset_particles_threshold; // find the best particle @@ -146,14 +155,14 @@ void Robot::resample_particles() } // determine resample interval area - double interval_x[2] = {0.0, 900.0}; - double interval_y[2] = {0.0, 600.0}; + int interval_x[2] = {0, 900}; + int interval_y[2] = {0, 600}; if (!projected_objects.empty() && !reset_particles && sum_weight > 0) { - interval_x[0] = position.x - 75; - interval_x[1] = position.x + 75; - interval_y[0] = position.y - 75; - interval_y[1] = position.y + 75; + interval_x[0] = position.x - 125; + interval_x[1] = position.x + 125; + interval_y[0] = position.y - 125; + interval_y[1] = position.y + 125; current_resample_interval = ResampleInterval::CENTER; } else { if (projected_objects.empty()) { @@ -165,9 +174,19 @@ void Robot::resample_particles() } } + // resample particles (debug) + // std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); + // std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); + + // for (int i = 0; i < num_particles; ++i) { + // particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + // particles[i].orientation = orientation; + // } + // resample particles std::uniform_real_distribution rand_prob(0.0, 1.0); double uniform_weight = 1.0 / num_particles; + for (int i = 0; i < num_particles; ++i) { if (rand_prob(rand_gen) < prob) { std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); @@ -187,6 +206,8 @@ void Robot::resample_particles() } } + particles = new_particles; + reset_particles = false; } @@ -301,7 +322,7 @@ double Robot::calculate_object_likelihood( (pow((landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + pow((landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); - likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * 100; + likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y); if (likelihood > current_likelihood) { current_likelihood = likelihood; @@ -390,9 +411,14 @@ void Robot::print_particles() { break; } + printf("Particles num: %d\n", particles.size()); printf("Resample interval: %s\n", resample_interval.c_str()); - printf("Prob: %.2f | is more than 0.1: %s\n", prob, (prob > 0.1) ? "true" : "false"); + printf("Prob: %.2f | is more than %.2f: %s\n", prob, (prob > reset_particles_threshold) ? "true" : "false", reset_particles_threshold); printf("Reset particles: %s\n", reset_particles ? "true" : "false"); + printf("Short term avg: %.5f\n", short_term_avg); + printf("Long term avg: %.5f\n", long_term_avg); + printf("Last weight avg: %.5f\n", last_weight_avg); + printf("Weight avg: %.5f\n", weight_avg); printf("========================================\n"); printf("Minimal Centered Particles: %.0f\n", min_centered_particles_ratio * num_particles); diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index f8c3781..fff9b7e 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -100,8 +100,8 @@ LocomotionNode::LocomotionNode( this->robot->projected_objects.clear(); for (const auto & obj : message->projected_objects) { if (obj.label == "ball" || obj.label == "robot" || obj.label == "self" || - obj.position.x < 0.0 || obj.position.x > 7.5 || - obj.position.y < -5.0 || obj.position.y > 5.0) { + obj.position.x < 0.0 || obj.position.x > 4.0 || + obj.position.y < -4.0 || obj.position.y > 4.0) { continue; } @@ -117,7 +117,6 @@ LocomotionNode::LocomotionNode( if (!this->robot->projected_objects.empty() && this->robot->use_localization) { printf("localize\n"); this->robot->localize(); - publish_particles(); } }); @@ -138,6 +137,7 @@ void LocomotionNode::update() } this->robot->apply_localization = false; publish_odometry(); + publish_particles(); } } From abdf34cfc0a9c57a42a975f0b1efad3f09949920 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 23 Mar 2025 12:03:16 +0700 Subject: [PATCH 34/45] feat: add max object distance config --- include/suiryoku/locomotion/model/robot.hpp | 1 + src/suiryoku/locomotion/node/locomotion_node.cpp | 9 ++++++--- src/suiryoku/locomotion/process/locomotion.cpp | 2 ++ 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 99a5483..204ae46 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -92,6 +92,7 @@ class Robot // IPM std::vector projected_objects; int num_projected_objects; + keisan::Point2 max_object_distance; // member for getting bool is_calibrated; diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index fff9b7e..1937120 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -99,9 +99,12 @@ LocomotionNode::LocomotionNode( [this](const ProjectedObjects::SharedPtr message) { this->robot->projected_objects.clear(); for (const auto & obj : message->projected_objects) { - if (obj.label == "ball" || obj.label == "robot" || obj.label == "self" || - obj.position.x < 0.0 || obj.position.x > 4.0 || - obj.position.y < -4.0 || obj.position.y > 4.0) { + bool ignore_object = obj.label == "ball" || obj.label == "robot" || obj.label == "self"; + ignore_object |= obj.position.x < 0.0; + ignore_object |= obj.position.x > this->robot->max_object_distance.x; + ignore_object |= obj.position.y < -this->robot->max_object_distance.y; + ignore_object |= obj.position.y > this->robot->max_object_distance.y; + if (ignore_object) { continue; } diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 8ac0569..91044d4 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -299,6 +299,8 @@ void Locomotion::set_config(const nlohmann::json & json) valid_section &= jitsuyo::assign_val(localization_section, "reset_particles_threshold", robot->reset_particles_threshold); valid_section &= jitsuyo::assign_val(localization_section, "sigma_x", robot->sigma_x); valid_section &= jitsuyo::assign_val(localization_section, "sigma_y", robot->sigma_y); + valid_section &= jitsuyo::assign_val(localization_section, "max_distance_x", robot->max_object_distance.x); + valid_section &= jitsuyo::assign_val(localization_section, "max_distance_y", robot->max_object_distance.y); if (!valid_section) { std::cout << "Error found at section `localization`" << std::endl; From 967f3045b384e30b3e6af0bbae8c02416b0366b2 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 23 Mar 2025 12:44:06 +0700 Subject: [PATCH 35/45] fix: resolve resampling problem --- include/suiryoku/locomotion/model/robot.hpp | 11 +- src/suiryoku/locomotion/model/robot.cpp | 217 +++++++++--------- .../locomotion/node/locomotion_node.cpp | 21 +- 3 files changed, 134 insertions(+), 115 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 204ae46..a23e5be 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -52,6 +52,13 @@ enum ResampleInterval FIELD_ZERO_WEIGHT }; +enum UpdateMotionState +{ + NOT_UPDATED, + WITH_NOISE, + WITHOUT_NOISE +}; + class Robot { public: @@ -124,7 +131,7 @@ class Robot double xvar; double yvar; int kidnap_counter; - Particle best_particle; + Particle* best_particle; std::mt19937 rand_gen; double weight_avg; @@ -135,7 +142,7 @@ class Robot bool initial_localization; int current_resample_interval; - int too_low_particles_count; + int update_motion_state; double prob; // for debug, remove later }; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 7e0297e..46ad20d 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -35,11 +35,11 @@ Robot::Robot() y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), is_calibrated(false), use_localization(false), apply_localization(false), - num_particles(500), xvar(10.0), yvar(10.0), kidnap_counter(0), weight_avg(0.0), - rand_gen(std::random_device{}()), short_term_avg(0.0), long_term_avg(0.0), + num_particles(500), xvar(10.0), yvar(10.0), kidnap_counter(0), best_particle(nullptr), + weight_avg(0.0), rand_gen(std::random_device{}()), short_term_avg(0.0), long_term_avg(0.0), last_weight_avg(0.0), estimated_position(0.0, 0.0), initial_localization(true), - reset_particles(false), too_low_particles_count(0), num_projected_objects(0), - sigma_x(1.0), sigma_y(1.0), short_term_avg_ratio(0.1), long_term_avg_ratio(0.001) + reset_particles(false), num_projected_objects(0), sigma_x(1.0), sigma_y(1.0), + short_term_avg_ratio(0.1), long_term_avg_ratio(0.001) { } @@ -81,10 +81,11 @@ void Robot::localize() short_term_avg += short_term_avg_ratio * (weight_avg - short_term_avg); long_term_avg += long_term_avg_ratio * (weight_avg - long_term_avg); + resample_particles(); + estimate_position(); } - estimate_position(); projected_objects.clear(); } @@ -95,13 +96,12 @@ void Robot::reset_localization() { center_particles.clear(); projected_objects.clear(); estimated_position = keisan::Point2(0.0, 0.0); - best_particle = Particle(); + best_particle = nullptr; weight_avg = 0.0; short_term_avg = 0.0; long_term_avg = 0.0; last_weight_avg = 0.0; current_resample_interval = ResampleInterval::FIELD_INIT; - too_low_particles_count = 0; reset_particles = false; prob = 0.0; @@ -115,8 +115,6 @@ void Robot::init_particles() double uniform_weight = 1.0 / num_particles; particles.clear(); particles.resize(num_particles); - // std::uniform_int_distribution xrg(position.x - 100, position.x + 100); // use if initial position is known - // std::uniform_int_distribution yrg(position.y - 100, position.y + 100); std::uniform_int_distribution xrg(0, 900); std::uniform_int_distribution yrg(0, 600); current_resample_interval = ResampleInterval::FIELD_INIT; @@ -130,28 +128,24 @@ void Robot::init_particles() void Robot::resample_particles() { - std::vector& old_particles = particles; std::vector new_particles(num_particles); - Particle current_best_particle; std::uniform_int_distribution rand_index(0, num_particles - 1); int index = rand_index(rand_gen); double beta = 0.0; double max_weight = 0.0; double sum_weight = get_sum_weight(); - // if (long_term_avg < 1e-6) long_term_avg = 1e-6; - prob = std::min(1.0, std::max(0.0, 1.0 - short_term_avg / long_term_avg)); + prob = std::max(0.0, 1.0 - short_term_avg / long_term_avg); reset_particles = prob > reset_particles_threshold; // find the best particle if (sum_weight > 0) { - for (auto & p : old_particles) { + for (auto & p : particles) { if (p.weight > max_weight) { max_weight = p.weight; - current_best_particle = p; + best_particle = &p; } } - best_particle = current_best_particle; } // determine resample interval area @@ -159,10 +153,11 @@ void Robot::resample_particles() int interval_y[2] = {0, 600}; if (!projected_objects.empty() && !reset_particles && sum_weight > 0) { - interval_x[0] = position.x - 125; - interval_x[1] = position.x + 125; - interval_y[0] = position.y - 125; - interval_y[1] = position.y + 125; + interval_x[0] = position.x - 100; + interval_x[1] = position.x + 100; + interval_y[0] = position.y - 100; + interval_y[1] = position.y + 100; + prob = std::min(0.05, prob); current_resample_interval = ResampleInterval::CENTER; } else { if (projected_objects.empty()) { @@ -174,35 +169,49 @@ void Robot::resample_particles() } } - // resample particles (debug) - // std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); - // std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); - - // for (int i = 0; i < num_particles; ++i) { - // particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); - // particles[i].orientation = orientation; - // } - // resample particles std::uniform_real_distribution rand_prob(0.0, 1.0); + std::uniform_real_distribution rand_prob2(0.0, 1.0); + std::uniform_real_distribution rand_beta(0.0, 2.0 * max_weight); + std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); + std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); + std::uniform_int_distribution field_xrg(0, 900); + std::uniform_int_distribution field_yrg(0, 600); double uniform_weight = 1.0 / num_particles; - for (int i = 0; i < num_particles; ++i) { - if (rand_prob(rand_gen) < prob) { - std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); - std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); - new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); - new_particles[i].orientation = orientation; - new_particles[i].weight = uniform_weight; - } else { - std::uniform_real_distribution rand_beta(0.0, 2.0 * max_weight); - beta += rand_beta(rand_gen); + // adaptive noise + double max_sigma = 35.0; + double min_sigma = 5.0; + + double sigma_factor = 1.0 - prob; + double resample_sigma_x = min_sigma + sigma_factor * (max_sigma - min_sigma); + double resample_sigma_y = min_sigma + sigma_factor * (max_sigma - min_sigma); - while (beta > old_particles[index].weight) { - beta -= old_particles[index].weight; - index = (index + 1) % num_particles; + std::normal_distribution noise_x(0.0, resample_sigma_x); + std::normal_distribution noise_y(0.0, resample_sigma_y); + + for (int i = 0; i < num_particles; ++i) { + if (rand_prob2(rand_gen) < 0.7) { + if (rand_prob(rand_gen) < prob) { + new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + new_particles[i].orientation = orientation; + new_particles[i].weight = uniform_weight; + } else { + beta += rand_beta(rand_gen); + + while (beta > particles[index].weight) { + beta -= particles[index].weight; + index = (index + 1) % num_particles; + } + new_particles[i] = particles[index]; + + new_particles[i].position.x += noise_x(rand_gen); + new_particles[i].position.y += noise_y(rand_gen); } - new_particles[i] = old_particles[index]; + } else { + new_particles[i].position = keisan::Point2(field_xrg(rand_gen), field_yrg(rand_gen)); + new_particles[i].orientation = orientation; + new_particles[i].weight = 0.0; } } @@ -213,7 +222,7 @@ void Robot::resample_particles() void Robot::update_motion() { - static std::normal_distribution<> xgen(0.0, xvar), ygen(0.0, yvar); + static std::normal_distribution xgen(0.0, xvar), ygen(0.0, yvar); if (!projected_objects.empty()) { for (auto & p : particles) { @@ -228,18 +237,14 @@ void Robot::update_motion() p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; p.orientation = orientation; } + update_motion_state = UpdateMotionState::WITH_NOISE; } else { - if (center_particles.empty()) { - best_particle.position = position + delta_position; - best_particle.orientation = orientation; - return; - } - - best_particle.position += delta_position; for (auto & p : particles) { - p.position += delta_position; + p.position.x += delta_position.x; + p.position.y += delta_position.y; p.orientation = orientation; } + update_motion_state = UpdateMotionState::WITHOUT_NOISE; } } @@ -254,19 +259,30 @@ double Robot::get_sum_weight() void Robot::calculate_weight() { + double sum_weight = 0.0; + for (auto & p : particles) { p.weight = calculate_total_likelihood(p); + sum_weight += p.weight; } // normalize weights - double sum_weight = get_sum_weight(); - if (sum_weight > 0 && !std::isnan(sum_weight)) { - for (auto & p : particles) { - p.weight /= sum_weight; - } - } + // if (!std::isnan(sum_weight) && sum_weight > 0) { + // for (auto & p : particles) { + // p.weight /= sum_weight; + // } + + // weight_avg = 1.0 / num_particles; + // } else { + // weight_avg = 0.0; + // } - weight_avg = sum_weight / num_particles; + // without normalize + if (!std::isnan(sum_weight) && sum_weight > 0) { + weight_avg = sum_weight / num_particles; + } else { + weight_avg = 0.0; + } } double Robot::calculate_total_likelihood(const Particle & particle) { @@ -285,8 +301,6 @@ double Robot::calculate_object_likelihood( double relative_position_x, relative_position_y; double dx, dy, x_rot, y_rot, exponent, likelihood; double current_likelihood = 0.0; - // keisan::Angle fov = 78.0_deg; - // keisan::Angle camera_orientation = orientation /* - get_pan() */; if (measurement.label == "L-Intersection") { landmarks = field.landmarks_L; @@ -299,15 +313,6 @@ double Robot::calculate_object_likelihood( } for (int i = 0; i < landmarks.size(); i++) { - // check if the landmark is in the camera field of view - // keisan::Angle angle_to_landmark = keisan::signed_arctan( - // landmarks[i].y - particle.position.y, landmarks[i].x - particle.position.x); - // keisan::Angle angle_diff = angle_to_landmark - camera_orientation; - - // if (std::fabs(angle_diff.degree()) > fov.degree() / 2) { - // continue; - // } - dx = measurement.position.x * 100; dy = measurement.position.y * 100; @@ -322,7 +327,7 @@ double Robot::calculate_object_likelihood( (pow((landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + pow((landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); - likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y); + likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * 100; if (likelihood > current_likelihood) { current_likelihood = likelihood; @@ -333,14 +338,18 @@ double Robot::calculate_object_likelihood( } void Robot::estimate_position() { + if (best_particle == nullptr || projected_objects.empty()) { + return; + } + keisan::Point2 sum_position = {0.0, 0.0}; int centered_particles = 0; center_particles.clear(); // Find numbers of particles closed to the best particle for (int i = 0; i < num_particles; ++i) { - double distance = sqrt(pow(particles[i].position.x - best_particle.position.x, 2) + - pow(particles[i].position.y - best_particle.position.y, 2)); + double distance = sqrt(pow(particles[i].position.x - best_particle->position.x, 2) + + pow(particles[i].position.y - best_particle->position.y, 2)); if (distance < 50.0) { centered_particles++; @@ -361,34 +370,10 @@ void Robot::estimate_position() { position = estimated_position; apply_localization = true; } - too_low_particles_count = 0; - } else { - too_low_particles_count++; - if (too_low_particles_count > 50) { - reset_particles = true; - too_low_particles_count = 0; - } } } void Robot::print_particles() { - double sum_samples = 0.0; - - for (int i = 0; i < num_particles; ++i) { - if (particles[i].weight > 0.0001) { - // std::cout << "Particle " << std::setw(5) << i - // << " weight: " << std::fixed << std::setprecision(5) - // << particles[i].weight << std::setw(5) << " [" - // << std::fixed << std::setprecision(2) << particles[i].position.x - // << ", " << std::fixed << std::setprecision(2) - // << particles[i].position.y << ", " << std::fixed - // << std::setprecision(2) << particles[i].orientation.degree() << "]" - // << std::endl; - - sum_samples += particles[i].weight; - } - } - std::string resample_interval; switch (current_resample_interval) { case ResampleInterval::CENTER: @@ -411,8 +396,26 @@ void Robot::print_particles() { break; } + std::string update_motion; + switch (update_motion_state) { + case UpdateMotionState::NOT_UPDATED: + update_motion = "NOT_UPDATED"; + break; + case UpdateMotionState::WITH_NOISE: + update_motion = "WITH_NOISE"; + break; + case UpdateMotionState::WITHOUT_NOISE: + update_motion = "WITHOUT_NOISE"; + break; + default: + update_motion = "UNKNOWN"; + break; + } + + printf("Particles num: %d\n", particles.size()); printf("Resample interval: %s\n", resample_interval.c_str()); + printf("Update motion: %s\n", update_motion.c_str()); printf("Prob: %.2f | is more than %.2f: %s\n", prob, (prob > reset_particles_threshold) ? "true" : "false", reset_particles_threshold); printf("Reset particles: %s\n", reset_particles ? "true" : "false"); printf("Short term avg: %.5f\n", short_term_avg); @@ -439,16 +442,20 @@ void Robot::print_particles() { } } - std::cout << "Best particle: " << std::fixed << std::setprecision(5) - << best_particle.weight << std::setw(5) << " [" - << std::fixed << std::setprecision(2) << best_particle.position.x - << ", " << std::fixed << std::setprecision(2) - << best_particle.position.y << ", " << std::fixed - << std::setprecision(2) << best_particle.orientation.degree() << "]" - << std::endl; + if (best_particle == nullptr) { + std::cout << "Best particle: NULL" << std::endl; + } else { + std::cout << "Best particle: " << std::fixed << std::setprecision(5) + << best_particle->weight << std::setw(5) << " [" + << std::fixed << std::setprecision(2) << best_particle->position.x + << ", " << std::fixed << std::setprecision(2) + << best_particle->position.y << ", " << std::fixed + << std::setprecision(2) << best_particle->orientation.degree() << "]" + << std::endl; + } std::cout << "Num particles: " << num_particles << std::endl; - std::cout << "Sum weights: " << sum_samples << std::endl; + std::cout << "Sum weights: " << get_sum_weight() << std::endl; std::cout << "Projected objects: " << num_projected_objects << std::endl; print_estimate_position(); } diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 1937120..75146cd 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -116,11 +116,6 @@ LocomotionNode::LocomotionNode( } this->robot->num_projected_objects = this->robot->projected_objects.size(); - - if (!this->robot->projected_objects.empty() && this->robot->use_localization) { - printf("localize\n"); - this->robot->localize(); - } }); locomotion->stop = [this]() {this->walking_state = false;}; @@ -129,9 +124,9 @@ LocomotionNode::LocomotionNode( void LocomotionNode::update() { - // printf("Starting localization\n"); - // this->robot->localize(); - // printf("Localization done\n"); + if (this->robot->use_localization) { + this->robot->localize(); + } publish_walking(); if (set_odometry || this->robot->apply_localization) { @@ -140,6 +135,9 @@ void LocomotionNode::update() } this->robot->apply_localization = false; publish_odometry(); + } + + if (this->robot->use_localization) { publish_particles(); } } @@ -171,6 +169,8 @@ void LocomotionNode::publish_odometry() void LocomotionNode::publish_particles() { Particles particles_msg; + double max_weight = 0.0; + for (const auto & p : robot->particles) { Particle particle_msg; particle_msg.x = p.position.x; @@ -179,8 +179,13 @@ void LocomotionNode::publish_particles() particle_msg.weight = p.weight; particles_msg.particles.push_back(particle_msg); + + if (p.weight > max_weight) { + max_weight = p.weight; + } } + particles_msg.max_weight = max_weight; particles_publisher->publish(particles_msg); } From 3706868da018f4be6adf546d5daf2fbba8b9b3eb Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 23 Mar 2025 15:14:03 +0700 Subject: [PATCH 36/45] fix: change likelihood calculation from product to sum --- src/suiryoku/locomotion/model/robot.cpp | 43 ++++++++----------------- 1 file changed, 14 insertions(+), 29 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 46ad20d..51fc869 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -90,7 +90,7 @@ void Robot::localize() } void Robot::reset_localization() { - apply_localization = false; + apply_localization = true; initial_localization = true; particles.clear(); center_particles.clear(); @@ -153,10 +153,10 @@ void Robot::resample_particles() int interval_y[2] = {0, 600}; if (!projected_objects.empty() && !reset_particles && sum_weight > 0) { - interval_x[0] = position.x - 100; - interval_x[1] = position.x + 100; - interval_y[0] = position.y - 100; - interval_y[1] = position.y + 100; + interval_x[0] = position.x - 75; + interval_x[1] = position.x + 75; + interval_y[0] = position.y - 75; + interval_y[1] = position.y + 75; prob = std::min(0.05, prob); current_resample_interval = ResampleInterval::CENTER; } else { @@ -175,13 +175,13 @@ void Robot::resample_particles() std::uniform_real_distribution rand_beta(0.0, 2.0 * max_weight); std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); - std::uniform_int_distribution field_xrg(0, 900); - std::uniform_int_distribution field_yrg(0, 600); + std::uniform_int_distribution large_xrg(interval_x[0] - 25, interval_x[1] + 25); + std::uniform_int_distribution large_yrg(interval_y[0] - 25, interval_y[1] + 25); double uniform_weight = 1.0 / num_particles; // adaptive noise - double max_sigma = 35.0; - double min_sigma = 5.0; + double max_sigma = 30.0; + double min_sigma = 1.0; double sigma_factor = 1.0 - prob; double resample_sigma_x = min_sigma + sigma_factor * (max_sigma - min_sigma); @@ -191,7 +191,7 @@ void Robot::resample_particles() std::normal_distribution noise_y(0.0, resample_sigma_y); for (int i = 0; i < num_particles; ++i) { - if (rand_prob2(rand_gen) < 0.7) { + if (rand_prob2(rand_gen) < 0.97) { if (rand_prob(rand_gen) < prob) { new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); new_particles[i].orientation = orientation; @@ -209,7 +209,7 @@ void Robot::resample_particles() new_particles[i].position.y += noise_y(rand_gen); } } else { - new_particles[i].position = keisan::Point2(field_xrg(rand_gen), field_yrg(rand_gen)); + new_particles[i].position = keisan::Point2(large_xrg(rand_gen), large_yrg(rand_gen)); new_particles[i].orientation = orientation; new_particles[i].weight = 0.0; } @@ -266,18 +266,6 @@ void Robot::calculate_weight() sum_weight += p.weight; } - // normalize weights - // if (!std::isnan(sum_weight) && sum_weight > 0) { - // for (auto & p : particles) { - // p.weight /= sum_weight; - // } - - // weight_avg = 1.0 / num_particles; - // } else { - // weight_avg = 0.0; - // } - - // without normalize if (!std::isnan(sum_weight) && sum_weight > 0) { weight_avg = sum_weight / num_particles; } else { @@ -286,9 +274,9 @@ void Robot::calculate_weight() } double Robot::calculate_total_likelihood(const Particle & particle) { - double total_likelihood = 1.0; + double total_likelihood = 0.0; for (const auto & object_measurement : projected_objects) { - total_likelihood *= + total_likelihood += calculate_object_likelihood(object_measurement, particle); } @@ -329,9 +317,7 @@ double Robot::calculate_object_likelihood( likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * 100; - if (likelihood > current_likelihood) { - current_likelihood = likelihood; - } + current_likelihood += likelihood; } return current_likelihood; @@ -412,7 +398,6 @@ void Robot::print_particles() { break; } - printf("Particles num: %d\n", particles.size()); printf("Resample interval: %s\n", resample_interval.c_str()); printf("Update motion: %s\n", update_motion.c_str()); From ceea0fe5010a44eb7794c80aac4c5f8b430245fc Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Sun, 30 Mar 2025 17:31:35 +0700 Subject: [PATCH 37/45] feat: publish estimated pose to particles topic --- src/suiryoku/locomotion/model/robot.cpp | 1 + src/suiryoku/locomotion/node/locomotion_node.cpp | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 51fc869..010f5a5 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -244,6 +244,7 @@ void Robot::update_motion() p.position.y += delta_position.y; p.orientation = orientation; } + estimated_position += delta_position; update_motion_state = UpdateMotionState::WITHOUT_NOISE; } } diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 75146cd..2c6ec65 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -185,7 +185,11 @@ void LocomotionNode::publish_particles() } } - particles_msg.max_weight = max_weight; + particles_msg.estimated_position.x = robot->estimated_position.x; + particles_msg.estimated_position.y = robot->estimated_position.y; + particles_msg.estimated_position.orientation = robot->orientation.degree(); + particles_msg.estimated_position.weight = max_weight; + particles_publisher->publish(particles_msg); } From 1aca1fb61272a3acf19ba4d38f1d68d9a53406c6 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Mon, 31 Mar 2025 07:58:08 +0700 Subject: [PATCH 38/45] fix: adding motion noise to motion update --- src/suiryoku/locomotion/model/robot.cpp | 61 +++++++++---------- .../locomotion/node/locomotion_node.cpp | 10 --- 2 files changed, 28 insertions(+), 43 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 010f5a5..bb7b755 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -175,12 +175,12 @@ void Robot::resample_particles() std::uniform_real_distribution rand_beta(0.0, 2.0 * max_weight); std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); - std::uniform_int_distribution large_xrg(interval_x[0] - 25, interval_x[1] + 25); - std::uniform_int_distribution large_yrg(interval_y[0] - 25, interval_y[1] + 25); + std::uniform_int_distribution large_xrg(0, 900); + std::uniform_int_distribution large_yrg(0, 600); double uniform_weight = 1.0 / num_particles; // adaptive noise - double max_sigma = 30.0; + double max_sigma = 10.0; double min_sigma = 1.0; double sigma_factor = 1.0 - prob; @@ -191,7 +191,7 @@ void Robot::resample_particles() std::normal_distribution noise_y(0.0, resample_sigma_y); for (int i = 0; i < num_particles; ++i) { - if (rand_prob2(rand_gen) < 0.97) { + if (rand_prob2(rand_gen) < 0.85) { if (rand_prob(rand_gen) < prob) { new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); new_particles[i].orientation = orientation; @@ -211,7 +211,7 @@ void Robot::resample_particles() } else { new_particles[i].position = keisan::Point2(large_xrg(rand_gen), large_yrg(rand_gen)); new_particles[i].orientation = orientation; - new_particles[i].weight = 0.0; + new_particles[i].weight = 1 / num_particles; } } @@ -224,29 +224,20 @@ void Robot::update_motion() { static std::normal_distribution xgen(0.0, xvar), ygen(0.0, yvar); - if (!projected_objects.empty()) { - for (auto & p : particles) { - double static_noise_x = xgen(rand_gen) / 5.0; - double static_noise_y = ygen(rand_gen) / 5.0; - double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; - double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; - double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; - double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0; - - p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; - p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; - p.orientation = orientation; - } - update_motion_state = UpdateMotionState::WITH_NOISE; - } else { - for (auto & p : particles) { - p.position.x += delta_position.x; - p.position.y += delta_position.y; - p.orientation = orientation; - } - estimated_position += delta_position; - update_motion_state = UpdateMotionState::WITHOUT_NOISE; + for (auto & p : particles) { + double static_noise_x = xgen(rand_gen) / 5.0; + double static_noise_y = ygen(rand_gen) / 5.0; + double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; + double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; + double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; + double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0; + + p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; + p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; + p.orientation = orientation; } + + estimated_position += delta_position; } double Robot::get_sum_weight() @@ -269,6 +260,9 @@ void Robot::calculate_weight() if (!std::isnan(sum_weight) && sum_weight > 0) { weight_avg = sum_weight / num_particles; + for (auto & p : particles) { + p.weight /= sum_weight; + } } else { weight_avg = 0.0; } @@ -316,7 +310,7 @@ double Robot::calculate_object_likelihood( (pow((landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + pow((landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); - likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * 100; + likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y); current_likelihood += likelihood; } @@ -352,11 +346,11 @@ void Robot::estimate_position() { estimated_position.x = sum_position.x / centered_particles; estimated_position.y = sum_position.y / centered_particles; - if (estimated_position.x >= 0.0 && estimated_position.x <= 900.0 && - estimated_position.y >= 0.0 && estimated_position.y <= 600.0) { - position = estimated_position; - apply_localization = true; - } + estimated_position.x = keisan::clamp(estimated_position.x, 0.0, 900.0); + estimated_position.y = keisan::clamp(estimated_position.y, 0.0, 600.0); + + position = estimated_position; + apply_localization = true; } } @@ -408,6 +402,7 @@ void Robot::print_particles() { printf("Long term avg: %.5f\n", long_term_avg); printf("Last weight avg: %.5f\n", last_weight_avg); printf("Weight avg: %.5f\n", weight_avg); + printf("Orientation: %.2f\n", orientation.degree()); printf("========================================\n"); printf("Minimal Centered Particles: %.0f\n", min_centered_particles_ratio * num_particles); diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 2c6ec65..3be297f 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -82,16 +82,6 @@ LocomotionNode::LocomotionNode( [this](const Point2::SharedPtr message) { this->robot->delta_position.x = message->x; this->robot->delta_position.y = message->y; - - // bool run_localization = false; - // run_localization |= message->x != 0.0; - // run_localization |= message->y != 0.0; - // run_localization |= this->robot->a_speed != 0.0; - // run_localization &= this->robot->use_localization; - - // if (run_localization) { - // this->robot->localize(); - // } }); projected_objects_subscriber = node->create_subscription( From 750e8f37be616b8e7c67ad5497f3fbf089077db8 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Wed, 9 Apr 2025 23:25:36 +0700 Subject: [PATCH 39/45] feat: prevent particle evaluation when roll-pitch or pan-tilt out of bounds --- include/suiryoku/locomotion/model/robot.hpp | 2 + src/suiryoku/locomotion/model/robot.cpp | 40 +++++++++++++------ .../locomotion/node/locomotion_node.cpp | 2 + 3 files changed, 31 insertions(+), 13 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index a23e5be..84b4672 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -104,6 +104,8 @@ class Robot // member for getting bool is_calibrated; keisan::Angle orientation; + keisan::Angle orientation_roll; + keisan::Angle orientation_pitch; keisan::Point2 position; keisan::Point2 delta_position; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index bb7b755..1378189 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -32,11 +32,12 @@ namespace suiryoku Robot::Robot() : pan(0_deg), tilt(0_deg), pan_center(0_deg), tilt_center(0_deg), x_speed(0.0), - y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), orientation(0_deg), - position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), - is_calibrated(false), use_localization(false), apply_localization(false), - num_particles(500), xvar(10.0), yvar(10.0), kidnap_counter(0), best_particle(nullptr), - weight_avg(0.0), rand_gen(std::random_device{}()), short_term_avg(0.0), long_term_avg(0.0), + y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), position(0.0, 0.0), + orientation(0_deg), orientation_roll(0_deg), orientation_pitch(0_deg), + x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), is_calibrated(false), + use_localization(false), apply_localization(false), num_particles(500), + xvar(10.0), yvar(10.0), kidnap_counter(0), best_particle(nullptr), weight_avg(0.0), + rand_gen(std::random_device{}()), short_term_avg(0.0), long_term_avg(0.0), last_weight_avg(0.0), estimated_position(0.0, 0.0), initial_localization(true), reset_particles(false), num_projected_objects(0), sigma_x(1.0), sigma_y(1.0), short_term_avg_ratio(0.1), long_term_avg_ratio(0.001) @@ -65,7 +66,13 @@ void Robot::localize() update_motion(); } - if (!projected_objects.empty()){ + bool evaluate_particles = !projected_objects.empty(); + evaluate_particles &= fabs(orientation_roll.degree()) <= 5; + evaluate_particles &= fabs(orientation_pitch.degree()) <= 3; + evaluate_particles &= fabs(get_pan().degree()) <= 70; + evaluate_particles &= get_tilt().degree() > -40; + + if (evaluate_particles) { weight_avg = 0.0; calculate_weight(); if (weight_avg <= 0 || std::isnan(weight_avg)) { @@ -172,15 +179,18 @@ void Robot::resample_particles() // resample particles std::uniform_real_distribution rand_prob(0.0, 1.0); std::uniform_real_distribution rand_prob2(0.0, 1.0); + std::uniform_real_distribution rand_prob3(0.0, 1.0); std::uniform_real_distribution rand_beta(0.0, 2.0 * max_weight); std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); - std::uniform_int_distribution large_xrg(0, 900); - std::uniform_int_distribution large_yrg(0, 600); + std::uniform_int_distribution large_xrg(interval_x[0] - 75, interval_x[1] + 75); + std::uniform_int_distribution large_yrg(interval_y[0] - 75, interval_y[1] + 75); + std::uniform_int_distribution field_xrg(0, 900); + std::uniform_int_distribution field_yrg(0, 600); double uniform_weight = 1.0 / num_particles; // adaptive noise - double max_sigma = 10.0; + double max_sigma = 15.0; double min_sigma = 1.0; double sigma_factor = 1.0 - prob; @@ -191,7 +201,7 @@ void Robot::resample_particles() std::normal_distribution noise_y(0.0, resample_sigma_y); for (int i = 0; i < num_particles; ++i) { - if (rand_prob2(rand_gen) < 0.85) { + if (rand_prob2(rand_gen) < 0.90) { if (rand_prob(rand_gen) < prob) { new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); new_particles[i].orientation = orientation; @@ -209,9 +219,13 @@ void Robot::resample_particles() new_particles[i].position.y += noise_y(rand_gen); } } else { - new_particles[i].position = keisan::Point2(large_xrg(rand_gen), large_yrg(rand_gen)); + if (rand_prob3 (rand_gen) < 0.5) { + new_particles[i].position = keisan::Point2(large_xrg(rand_gen), large_yrg(rand_gen)); + } else { + new_particles[i].position = keisan::Point2(field_xrg(rand_gen), field_yrg(rand_gen)); + } new_particles[i].orientation = orientation; - new_particles[i].weight = 1 / num_particles; + new_particles[i].weight = 0.1 / num_particles; } } @@ -310,7 +324,7 @@ double Robot::calculate_object_likelihood( (pow((landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + pow((landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); - likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y); + likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * 100.0; current_likelihood += likelihood; } diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index 3be297f..a3bdc84 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -50,6 +50,8 @@ LocomotionNode::LocomotionNode( [this](const MeasurementStatus::SharedPtr message) { this->robot->is_calibrated = message->is_calibrated; this->robot->orientation = keisan::make_degree(message->orientation.yaw); + this->robot->orientation_roll = keisan::make_degree(message->orientation.roll); + this->robot->orientation_pitch = keisan::make_degree(message->orientation.pitch); }); set_odometry_publisher = node->create_publisher( From 058d455ee405d7b020f5132ae01e2f98d67ccb8e Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Wed, 23 Apr 2025 22:03:17 +0700 Subject: [PATCH 40/45] fix: resample and initial particles --- include/suiryoku/locomotion/model/robot.hpp | 2 +- src/suiryoku/locomotion/model/robot.cpp | 133 +++++++++--------- .../locomotion/node/locomotion_node.cpp | 15 +- 3 files changed, 73 insertions(+), 77 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 84b4672..c2628e7 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -70,7 +70,7 @@ class Robot // localizations void localize(); void reset_localization(); - void init_particles(); + void init_particles(const keisan::Point2 init_position); void resample_particles(); void update_motion(); void calculate_weight(); diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 1378189..8cf43e1 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -61,16 +61,16 @@ void Robot::localize() } if (initial_localization) { - init_particles(); + init_particles(keisan::Point2(-1, -1)); } else { update_motion(); } bool evaluate_particles = !projected_objects.empty(); - evaluate_particles &= fabs(orientation_roll.degree()) <= 5; - evaluate_particles &= fabs(orientation_pitch.degree()) <= 3; - evaluate_particles &= fabs(get_pan().degree()) <= 70; - evaluate_particles &= get_tilt().degree() > -40; + evaluate_particles &= fabs(orientation_roll.degree()) <= 25; + evaluate_particles &= fabs(orientation_pitch.degree()) <= 25; + evaluate_particles &= fabs(get_pan().degree()) <= 80; + evaluate_particles &= get_tilt().degree() > -45; if (evaluate_particles) { weight_avg = 0.0; @@ -78,20 +78,23 @@ void Robot::localize() if (weight_avg <= 0 || std::isnan(weight_avg)) { weight_avg = last_weight_avg; } else { - if (initial_localization) { + if (initial_localization || reset_particles) { short_term_avg = weight_avg; long_term_avg = weight_avg; initial_localization = false; + reset_particles = false; } last_weight_avg = weight_avg; } - short_term_avg += short_term_avg_ratio * (weight_avg - short_term_avg); - long_term_avg += long_term_avg_ratio * (weight_avg - long_term_avg); + if (!std::isnan(weight_avg)) { + short_term_avg += short_term_avg_ratio * (weight_avg - short_term_avg); + long_term_avg += long_term_avg_ratio * (weight_avg - long_term_avg); + } resample_particles(); - estimate_position(); } + estimate_position(); projected_objects.clear(); } @@ -117,19 +120,31 @@ void Robot::reset_localization() { } } -void Robot::init_particles() +void Robot::init_particles(const keisan::Point2 init_position) { double uniform_weight = 1.0 / num_particles; particles.clear(); particles.resize(num_particles); - std::uniform_int_distribution xrg(0, 900); - std::uniform_int_distribution yrg(0, 600); - current_resample_interval = ResampleInterval::FIELD_INIT; - for (int i = 0; i < num_particles; ++i) { - particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); - particles[i].orientation = orientation; - particles[i].weight = uniform_weight; + if (init_position.x == -1 && init_position.y == -1) { + std::uniform_int_distribution xrg(0, 900); + std::uniform_int_distribution yrg(0, 600); + current_resample_interval = ResampleInterval::FIELD_INIT; + + for (int i = 0; i < num_particles; ++i) { + particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + particles[i].orientation = orientation; + particles[i].weight = uniform_weight; + } + } else { + std::normal_distribution xrg(init_position.x, 25); + std::normal_distribution yrg(init_position.y, 25); + + for (int i = 0; i < num_particles; ++i) { + particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + particles[i].orientation = orientation; + particles[i].weight = uniform_weight; + } } } @@ -156,42 +171,31 @@ void Robot::resample_particles() } // determine resample interval area - int interval_x[2] = {0, 900}; - int interval_y[2] = {0, 600}; + int interval_x[2] = {10, 890}; + int interval_y[2] = {10, 590}; + + bool resample_nearby = num_projected_objects == 1; + resample_nearby |= fabs(get_pan().degree()) > 50; + resample_nearby |= get_tilt().degree() < -30; - if (!projected_objects.empty() && !reset_particles && sum_weight > 0) { + if (resample_nearby) { interval_x[0] = position.x - 75; interval_x[1] = position.x + 75; interval_y[0] = position.y - 75; interval_y[1] = position.y + 75; prob = std::min(0.05, prob); current_resample_interval = ResampleInterval::CENTER; - } else { - if (projected_objects.empty()) { - current_resample_interval = ResampleInterval::FIELD_EMPTY_PROJECTED_OBJECTS; - } else if (reset_particles) { - current_resample_interval = ResampleInterval::FIELD_RESET; - } else if (sum_weight <= 0) { - current_resample_interval = ResampleInterval::FIELD_ZERO_WEIGHT; - } } // resample particles std::uniform_real_distribution rand_prob(0.0, 1.0); - std::uniform_real_distribution rand_prob2(0.0, 1.0); - std::uniform_real_distribution rand_prob3(0.0, 1.0); std::uniform_real_distribution rand_beta(0.0, 2.0 * max_weight); std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); - std::uniform_int_distribution large_xrg(interval_x[0] - 75, interval_x[1] + 75); - std::uniform_int_distribution large_yrg(interval_y[0] - 75, interval_y[1] + 75); - std::uniform_int_distribution field_xrg(0, 900); - std::uniform_int_distribution field_yrg(0, 600); - double uniform_weight = 1.0 / num_particles; // adaptive noise - double max_sigma = 15.0; - double min_sigma = 1.0; + double max_sigma = 5.0; + double min_sigma = 0.1; double sigma_factor = 1.0 - prob; double resample_sigma_x = min_sigma + sigma_factor * (max_sigma - min_sigma); @@ -201,37 +205,33 @@ void Robot::resample_particles() std::normal_distribution noise_y(0.0, resample_sigma_y); for (int i = 0; i < num_particles; ++i) { - if (rand_prob2(rand_gen) < 0.90) { - if (rand_prob(rand_gen) < prob) { - new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); - new_particles[i].orientation = orientation; - new_particles[i].weight = uniform_weight; - } else { - beta += rand_beta(rand_gen); - - while (beta > particles[index].weight) { - beta -= particles[index].weight; - index = (index + 1) % num_particles; - } - new_particles[i] = particles[index]; - - new_particles[i].position.x += noise_x(rand_gen); - new_particles[i].position.y += noise_y(rand_gen); - } + if (rand_prob(rand_gen) < prob) { + new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + new_particles[i].orientation = orientation; + new_particles[i].weight = 0.0; } else { - if (rand_prob3 (rand_gen) < 0.5) { - new_particles[i].position = keisan::Point2(large_xrg(rand_gen), large_yrg(rand_gen)); - } else { - new_particles[i].position = keisan::Point2(field_xrg(rand_gen), field_yrg(rand_gen)); + beta += rand_beta(rand_gen); + + while (beta > particles[index].weight) { + beta -= particles[index].weight; + index = (index + 1) % num_particles; } + new_particles[i] = particles[index]; + + new_particles[i].position.x += noise_x(rand_gen); + new_particles[i].position.y += noise_y(rand_gen); + } + + // check is particle out of bound + if (new_particles[i].position.x < 0.0 || new_particles[i].position.x > 900.0 || + new_particles[i].position.y < 0.0 || new_particles[i].position.y > 600.0) { + new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); new_particles[i].orientation = orientation; - new_particles[i].weight = 0.1 / num_particles; + new_particles[i].weight = 0.0; } } particles = new_particles; - - reset_particles = false; } void Robot::update_motion() @@ -324,9 +324,11 @@ double Robot::calculate_object_likelihood( (pow((landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + pow((landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); - likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * 100.0; + likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y); - current_likelihood += likelihood; + if (current_likelihood < likelihood) { + current_likelihood = likelihood; + } } return current_likelihood; @@ -346,7 +348,7 @@ void Robot::estimate_position() { double distance = sqrt(pow(particles[i].position.x - best_particle->position.x, 2) + pow(particles[i].position.y - best_particle->position.y, 2)); - if (distance < 50.0) { + if (distance < 25.0) { centered_particles++; sum_position.x += particles[i].position.x; sum_position.y += particles[i].position.y; @@ -355,7 +357,7 @@ void Robot::estimate_position() { } } - // Use mean of centered particles position if more than 30% of particles are centered + // Use mean of centered particles position if more than threshold are centered if (centered_particles > min_centered_particles_ratio * num_particles) { estimated_position.x = sum_position.x / centered_particles; estimated_position.y = sum_position.y / centered_particles; @@ -449,6 +451,7 @@ void Robot::print_particles() { << std::endl; } + std::cout << "Num objects: " << num_projected_objects << std::endl; std::cout << "Num particles: " << num_particles << std::endl; std::cout << "Sum weights: " << get_sum_weight() << std::endl; std::cout << "Projected objects: " << num_projected_objects << std::endl; diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index a3bdc84..c3951d3 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -92,10 +92,9 @@ LocomotionNode::LocomotionNode( this->robot->projected_objects.clear(); for (const auto & obj : message->projected_objects) { bool ignore_object = obj.label == "ball" || obj.label == "robot" || obj.label == "self"; - ignore_object |= obj.position.x < 0.0; - ignore_object |= obj.position.x > this->robot->max_object_distance.x; - ignore_object |= obj.position.y < -this->robot->max_object_distance.y; - ignore_object |= obj.position.y > this->robot->max_object_distance.y; + ignore_object |= obj.position.x * 100 > this->robot->max_object_distance.x; + ignore_object |= obj.position.y * 100 < -this->robot->max_object_distance.y; + ignore_object |= obj.position.y * 100 > this->robot->max_object_distance.y; if (ignore_object) { continue; } @@ -118,20 +117,14 @@ void LocomotionNode::update() { if (this->robot->use_localization) { this->robot->localize(); + publish_particles(); } publish_walking(); if (set_odometry || this->robot->apply_localization) { - if (this->robot->apply_localization) { - printf("Localization applied\n"); - } this->robot->apply_localization = false; publish_odometry(); } - - if (this->robot->use_localization) { - publish_particles(); - } } void LocomotionNode::publish_walking() From b957cd08fbe5e5f0e05923a244f66004736abd93 Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Thu, 24 Apr 2025 01:49:02 +0700 Subject: [PATCH 41/45] feat: use hungarian for landmark matching --- include/suiryoku/locomotion/model/robot.hpp | 17 ++- src/suiryoku/locomotion/model/robot.cpp | 123 ++++++++++++------ .../locomotion/node/locomotion_node.cpp | 24 ++-- 3 files changed, 114 insertions(+), 50 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index c2628e7..2d938a3 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -43,6 +43,11 @@ struct Particle double weight; }; +struct LandmarkGroup { + std::vector* projected; + std::vector* landmarks; +}; + enum ResampleInterval { CENTER, @@ -70,6 +75,7 @@ class Robot // localizations void localize(); void reset_localization(); + void clear_projected_objects(); void init_particles(const keisan::Point2 init_position); void resample_particles(); void update_motion(); @@ -78,9 +84,13 @@ class Robot void print_particles(); void print_estimate_position(); void set_initial_localization(bool initial) { initial_localization = initial; } + double calculate_landmark_cost(const keisan::Point2 & landmark, const keisan::Point2 & projected_object); double calculate_total_likelihood(const Particle & particle); - double calculate_object_likelihood(const ProjectedObject & measurement, const Particle & particle); + double calculate_object_likelihood(const ProjectedObject & measurement, const keisan::Point2 & landmark, const Particle & particle); double get_sum_weight(); + keisan::Matrix<6, 6> calculate_cost_matrix(const Particle & particle, + const std::vector & projected_objects, + const std::vector & landmarks); Field field; std::vector particles; @@ -97,7 +107,10 @@ class Robot bool apply_localization; // IPM - std::vector projected_objects; + std::vector projected_X; + std::vector projected_L; + std::vector projected_T; + std::vector projected_goalpost; int num_projected_objects; keisan::Point2 max_object_distance; diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 8cf43e1..d99346e 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -24,6 +24,7 @@ #include "suiryoku/locomotion/model/robot.hpp" #include "keisan/keisan.hpp" +#include "keisan/hungarian.hpp" using keisan::literals::operator""_deg; @@ -66,7 +67,7 @@ void Robot::localize() update_motion(); } - bool evaluate_particles = !projected_objects.empty(); + bool evaluate_particles = num_projected_objects > 0; evaluate_particles &= fabs(orientation_roll.degree()) <= 25; evaluate_particles &= fabs(orientation_pitch.degree()) <= 25; evaluate_particles &= fabs(get_pan().degree()) <= 80; @@ -96,7 +97,7 @@ void Robot::localize() } estimate_position(); - projected_objects.clear(); + clear_projected_objects(); } void Robot::reset_localization() { @@ -104,7 +105,7 @@ void Robot::reset_localization() { initial_localization = true; particles.clear(); center_particles.clear(); - projected_objects.clear(); + clear_projected_objects(); estimated_position = keisan::Point2(0.0, 0.0); best_particle = nullptr; weight_avg = 0.0; @@ -120,6 +121,15 @@ void Robot::reset_localization() { } } +void Robot::clear_projected_objects() +{ + projected_X.clear(); + projected_L.clear(); + projected_T.clear(); + projected_goalpost.clear(); + num_projected_objects = 0; +} + void Robot::init_particles(const keisan::Point2 init_position) { double uniform_weight = 1.0 / num_particles; @@ -282,60 +292,93 @@ void Robot::calculate_weight() } } +double Robot::calculate_landmark_cost(const keisan::Point2 & landmark, const keisan::Point2 & projected_object) { + double dx = landmark.x - projected_object.x; + double dy = landmark.y - projected_object.y; + + return sqrt(dx * dx + dy * dy); +} + +keisan::Matrix<6, 6> Robot::calculate_cost_matrix( + const Particle & particle, const std::vector & projected_objects, + const std::vector & landmarks) { + auto cost_matrix = keisan::Matrix<6, 6>::infinite(); + int num_projected_objects = projected_objects.size(); + int num_landmarks = landmarks.size(); + + for (int i = 0; i < num_projected_objects; ++i) { + for (int j = 0; j < num_landmarks; ++j) { + double dx = projected_objects[i].position.x * 100; + double dy = projected_objects[i].position.y * 100; + + double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); + double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); + + double relative_position_x = particle.position.x + x_rot; + double relative_position_y = particle.position.y + y_rot; + + keisan::Point2 projected_object(relative_position_x, relative_position_y); + + cost_matrix[i][j] = calculate_landmark_cost(landmarks[j], projected_object); + } + } + + return cost_matrix; +} + double Robot::calculate_total_likelihood(const Particle & particle) { + keisan::Hungarian<6> hungarian; double total_likelihood = 0.0; - for (const auto & object_measurement : projected_objects) { - total_likelihood += - calculate_object_likelihood(object_measurement, particle); + + std::vector landmark_groups = { + {&projected_X, &field.landmarks_X}, + {&projected_L, &field.landmarks_L}, + {&projected_T, &field.landmarks_T}, + {&projected_goalpost, &field.landmarks_goalpost} + }; + + for (const auto & group : landmark_groups) { + if (group.projected->empty() || group.landmarks->empty()) { + continue; + } + + auto cost_matrix = calculate_cost_matrix(particle, *group.projected, *group.landmarks); + auto result = hungarian.solve(cost_matrix, group.landmarks->size()); + + for (int i = 0; i < group.projected->size(); ++i) { + for (int j = 0; j < group.landmarks->size(); ++j) { + if (result[i][j] == 1) { + total_likelihood += calculate_object_likelihood((*group.projected)[i], (*group.landmarks)[j], particle); + } + } + } } return total_likelihood; } double Robot::calculate_object_likelihood( - const ProjectedObject & measurement, const Particle & particle) { - std::vector landmarks; - double relative_position_x, relative_position_y; - double dx, dy, x_rot, y_rot, exponent, likelihood; - double current_likelihood = 0.0; - - if (measurement.label == "L-Intersection") { - landmarks = field.landmarks_L; - } else if (measurement.label == "T-Intersection") { - landmarks = field.landmarks_T; - } else if (measurement.label == "X-Intersection") { - landmarks = field.landmarks_X; - } else if (measurement.label == "goalpost") { - landmarks = field.landmarks_goalpost; - } + const ProjectedObject & measurement, const keisan::Point2 & landmark, const Particle & particle) { - for (int i = 0; i < landmarks.size(); i++) { - dx = measurement.position.x * 100; - dy = measurement.position.y * 100; + double dx = measurement.position.x * 100; + double dy = measurement.position.y * 100; - x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); - y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); + double x_rot = dx * particle.orientation.cos() + dy * particle.orientation.sin(); + double y_rot = dx * particle.orientation.sin() - dy * particle.orientation.cos(); - relative_position_x = particle.position.x + x_rot; - relative_position_y = particle.position.y + y_rot; + double relative_position_x = particle.position.x + x_rot; + double relative_position_y = particle.position.y + y_rot; - exponent = + double exponent = -0.5 * - (pow((landmarks[i].x - relative_position_x), 2) / pow(sigma_x, 2) + - pow((landmarks[i].y - relative_position_y), 2) / pow(sigma_y, 2)); - - likelihood = exp(exponent) / (2 * M_PI * sigma_x * sigma_y); - - if (current_likelihood < likelihood) { - current_likelihood = likelihood; - } - } + (pow((landmark.x - relative_position_x), 2) / pow(sigma_x, 2) + + pow((landmark.y - relative_position_y), 2) / pow(sigma_y, 2)); - return current_likelihood; + return exp(exponent) / (2 * M_PI * sigma_x * sigma_y); } void Robot::estimate_position() { - if (best_particle == nullptr || projected_objects.empty()) { + if (best_particle == nullptr || num_projected_objects == 0) { return; } diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp index c3951d3..edeac0c 100644 --- a/src/suiryoku/locomotion/node/locomotion_node.cpp +++ b/src/suiryoku/locomotion/node/locomotion_node.cpp @@ -89,7 +89,7 @@ LocomotionNode::LocomotionNode( projected_objects_subscriber = node->create_subscription( "/gyakuenki_cpp/projected_objects", 10, [this](const ProjectedObjects::SharedPtr message) { - this->robot->projected_objects.clear(); + this->robot->clear_projected_objects(); for (const auto & obj : message->projected_objects) { bool ignore_object = obj.label == "ball" || obj.label == "robot" || obj.label == "self"; ignore_object |= obj.position.x * 100 > this->robot->max_object_distance.x; @@ -99,14 +99,22 @@ LocomotionNode::LocomotionNode( continue; } - this->robot->projected_objects.push_back( - ProjectedObject{ - obj.label, - keisan::Point3{obj.position.x, obj.position.y, obj.position.z} - }); + auto projected_object = ProjectedObject{ + obj.label, + keisan::Point3{obj.position.x, obj.position.y, obj.position.z} + }; + + if (obj.label == "X-Intersection") { + this->robot->projected_X.push_back(projected_object); + } else if (obj.label == "L-Intersection") { + this->robot->projected_L.push_back(projected_object); + } else if (obj.label == "T-Intersection") { + this->robot->projected_T.push_back(projected_object); + } else if (obj.label == "goalpost") { + this->robot->projected_goalpost.push_back(projected_object); + } + this->robot->num_projected_objects++; } - - this->robot->num_projected_objects = this->robot->projected_objects.size(); }); locomotion->stop = [this]() {this->walking_state = false;}; From ac5cc3b736568f874a67944a1cbd3a59d85c12ef Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Fri, 25 Apr 2025 23:33:21 +0700 Subject: [PATCH 42/45] refactor: remove unused debug code and tidy up --- include/suiryoku/locomotion/model/robot.hpp | 48 +++----- src/suiryoku/locomotion/model/robot.cpp | 106 +++++------------- .../locomotion/process/locomotion.cpp | 1 + 3 files changed, 49 insertions(+), 106 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 2d938a3..6e765a3 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -25,6 +25,7 @@ #include #include "keisan/keisan.hpp" +#include "keisan/hungarian.hpp" #include "suiryoku/locomotion/model/field.hpp" namespace suiryoku @@ -48,22 +49,6 @@ struct LandmarkGroup { std::vector* landmarks; }; -enum ResampleInterval -{ - CENTER, - FIELD_INIT, - FIELD_RESET, - FIELD_EMPTY_PROJECTED_OBJECTS, - FIELD_ZERO_WEIGHT -}; - -enum UpdateMotionState -{ - NOT_UPDATED, - WITH_NOISE, - WITHOUT_NOISE -}; - class Robot { public: @@ -84,35 +69,40 @@ class Robot void print_particles(); void print_estimate_position(); void set_initial_localization(bool initial) { initial_localization = initial; } + double calculate_landmark_cost(const keisan::Point2 & landmark, const keisan::Point2 & projected_object); double calculate_total_likelihood(const Particle & particle); - double calculate_object_likelihood(const ProjectedObject & measurement, const keisan::Point2 & landmark, const Particle & particle); + double calculate_object_likelihood(const ProjectedObject & measurement, + const keisan::Point2 & landmark, + const Particle & particle); + double get_sum_weight(); keisan::Matrix<6, 6> calculate_cost_matrix(const Particle & particle, const std::vector & projected_objects, const std::vector & landmarks); - Field field; std::vector particles; keisan::Point2 estimated_position; + int num_particles; double min_centered_particles_ratio; + double reset_particles_threshold; double short_term_avg_ratio; double long_term_avg_ratio; - double reset_particles_threshold; double sigma_x; double sigma_y; bool use_localization; bool apply_localization; + bool print_debug; // IPM std::vector projected_X; std::vector projected_L; std::vector projected_T; std::vector projected_goalpost; - int num_projected_objects; keisan::Point2 max_object_distance; + int num_projected_objects; // member for getting bool is_calibrated; @@ -139,27 +129,25 @@ class Robot double a_speed; bool aim_on; - bool reset_particles; // for debug, change to private later - std::vector center_particles; // for debug, erase later - private: + Field field; + Particle* best_particle; + std::vector center_particles; + double xvar; double yvar; - int kidnap_counter; - Particle* best_particle; std::mt19937 rand_gen; + keisan::Hungarian<6> hungarian; + double weight_avg; double short_term_avg; double long_term_avg; double last_weight_avg; + double prob; bool initial_localization; - - int current_resample_interval; - int update_motion_state; - - double prob; // for debug, remove later + bool reset_particles; }; } // namespace suiryoku diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index d99346e..eb95185 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -24,7 +24,6 @@ #include "suiryoku/locomotion/model/robot.hpp" #include "keisan/keisan.hpp" -#include "keisan/hungarian.hpp" using keisan::literals::operator""_deg; @@ -36,12 +35,13 @@ Robot::Robot() y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false), position(0.0, 0.0), orientation(0_deg), orientation_roll(0_deg), orientation_pitch(0_deg), x_amplitude(0.0), y_amplitude(0.0), a_amplitude(0.0), is_calibrated(false), - use_localization(false), apply_localization(false), num_particles(500), - xvar(10.0), yvar(10.0), kidnap_counter(0), best_particle(nullptr), weight_avg(0.0), - rand_gen(std::random_device{}()), short_term_avg(0.0), long_term_avg(0.0), - last_weight_avg(0.0), estimated_position(0.0, 0.0), initial_localization(true), - reset_particles(false), num_projected_objects(0), sigma_x(1.0), sigma_y(1.0), - short_term_avg_ratio(0.1), long_term_avg_ratio(0.001) + use_localization(false), apply_localization(false), print_debug(false), + num_particles(500), min_centered_particles_ratio(0.3), reset_particles_threshold(0.3), + short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), sigma_x(5.0), sigma_y(5.0), + xvar(10.0), yvar(10.0), best_particle(nullptr), rand_gen(std::random_device{}()), + weight_avg(0.0), short_term_avg(0.0), long_term_avg(0.0), last_weight_avg(0.0), + estimated_position(0.0, 0.0), initial_localization(true), reset_particles(false), + num_projected_objects(0), max_object_distance(300.0, 300.0) { } @@ -112,7 +112,6 @@ void Robot::reset_localization() { short_term_avg = 0.0; long_term_avg = 0.0; last_weight_avg = 0.0; - current_resample_interval = ResampleInterval::FIELD_INIT; reset_particles = false; prob = 0.0; @@ -139,7 +138,6 @@ void Robot::init_particles(const keisan::Point2 init_position) if (init_position.x == -1 && init_position.y == -1) { std::uniform_int_distribution xrg(0, 900); std::uniform_int_distribution yrg(0, 600); - current_resample_interval = ResampleInterval::FIELD_INIT; for (int i = 0; i < num_particles; ++i) { particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); @@ -194,7 +192,6 @@ void Robot::resample_particles() interval_y[0] = position.y - 75; interval_y[1] = position.y + 75; prob = std::min(0.05, prob); - current_resample_interval = ResampleInterval::CENTER; } // resample particles @@ -327,7 +324,6 @@ keisan::Matrix<6, 6> Robot::calculate_cost_matrix( } double Robot::calculate_total_likelihood(const Particle & particle) { - keisan::Hungarian<6> hungarian; double total_likelihood = 0.0; std::vector landmark_groups = { @@ -414,68 +410,33 @@ void Robot::estimate_position() { } void Robot::print_particles() { - std::string resample_interval; - switch (current_resample_interval) { - case ResampleInterval::CENTER: - resample_interval = "CENTER"; - break; - case ResampleInterval::FIELD_EMPTY_PROJECTED_OBJECTS: - resample_interval = "FIELD_EMPTY_PROJECTED_OBJECTS"; - break; - case ResampleInterval::FIELD_INIT: - resample_interval = "FIELD_INIT"; - break; - case ResampleInterval::FIELD_RESET: - resample_interval = "FIELD_RESET"; - break; - case ResampleInterval::FIELD_ZERO_WEIGHT: - resample_interval = "FIELD_ZERO_WEIGHT"; - break; - default: - resample_interval = "UNKNOWN"; - break; - } - - std::string update_motion; - switch (update_motion_state) { - case UpdateMotionState::NOT_UPDATED: - update_motion = "NOT_UPDATED"; - break; - case UpdateMotionState::WITH_NOISE: - update_motion = "WITH_NOISE"; - break; - case UpdateMotionState::WITHOUT_NOISE: - update_motion = "WITHOUT_NOISE"; - break; - default: - update_motion = "UNKNOWN"; - break; + if (!print_debug) { + return; } printf("Particles num: %d\n", particles.size()); - printf("Resample interval: %s\n", resample_interval.c_str()); - printf("Update motion: %s\n", update_motion.c_str()); - printf("Prob: %.2f | is more than %.2f: %s\n", prob, (prob > reset_particles_threshold) ? "true" : "false", reset_particles_threshold); + printf("Prob: %.2f | Threshold: %.2f | Above threshold: %s\n", + prob, + reset_particles_threshold, + (prob > reset_particles_threshold) ? "true" : "false"); printf("Reset particles: %s\n", reset_particles ? "true" : "false"); printf("Short term avg: %.5f\n", short_term_avg); printf("Long term avg: %.5f\n", long_term_avg); printf("Last weight avg: %.5f\n", last_weight_avg); printf("Weight avg: %.5f\n", weight_avg); - printf("Orientation: %.2f\n", orientation.degree()); + printf("Sum weights: %.5f\n", get_sum_weight()); printf("========================================\n"); printf("Minimal Centered Particles: %.0f\n", min_centered_particles_ratio * num_particles); printf("Centered particles: %d\n", center_particles.size()); for (int i = 0; i < center_particles.size(); ++i) { - std::cout << "Centered Particle " << std::setw(5) << i - << " weight: " << std::fixed << std::setprecision(5) - << center_particles[i]->weight << std::setw(5) << " [" - << std::fixed << std::setprecision(2) << center_particles[i]->position.x - << ", " << std::fixed << std::setprecision(2) - << center_particles[i]->position.y << ", " << std::fixed - << std::setprecision(2) << center_particles[i]->orientation.degree() << "]" - << std::endl; + printf("Centered Particle %d weight: %.5f [%.2f, %.2f, %.2f]\n", + i, + center_particles[i]->weight, + center_particles[i]->position.x, + center_particles[i]->position.y, + center_particles[i]->orientation.degree()); if (i >= 10) { printf("... and %d more\n", center_particles.size() - 10); break; @@ -483,30 +444,23 @@ void Robot::print_particles() { } if (best_particle == nullptr) { - std::cout << "Best particle: NULL" << std::endl; + printf("Best particle: NULL\n"); } else { - std::cout << "Best particle: " << std::fixed << std::setprecision(5) - << best_particle->weight << std::setw(5) << " [" - << std::fixed << std::setprecision(2) << best_particle->position.x - << ", " << std::fixed << std::setprecision(2) - << best_particle->position.y << ", " << std::fixed - << std::setprecision(2) << best_particle->orientation.degree() << "]" - << std::endl; + printf("Best particle: %.5f [%.2f, %.2f, %.2f]\n", + best_particle->weight, + best_particle->position.x, + best_particle->position.y, + best_particle->orientation.degree()); } - std::cout << "Num objects: " << num_projected_objects << std::endl; - std::cout << "Num particles: " << num_particles << std::endl; - std::cout << "Sum weights: " << get_sum_weight() << std::endl; - std::cout << "Projected objects: " << num_projected_objects << std::endl; + printf("Num objects: %d\n", num_projected_objects); + printf("Num particles: %d\n", num_particles); + printf("Projected objects: %d\n", num_projected_objects); print_estimate_position(); } void Robot::print_estimate_position() { - std::cout << "Pose estimation: " - << " [" << std::fixed << std::setprecision(2) - << estimated_position.x << ", " << std::fixed - << std::setprecision(2) << estimated_position.y - << "])" << std::endl; + printf("Pose estimation: [%.2f, %.2f])\n", estimated_position.x, estimated_position.y); } } // namespace suiryoku diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 91044d4..502dbec 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -292,6 +292,7 @@ void Locomotion::set_config(const nlohmann::json & json) bool valid_section = true; valid_section &= jitsuyo::assign_val(localization_section, "enable", robot->use_localization); + valid_section &= jitsuyo::assign_val(localization_section, "debug", robot->print_debug); valid_section &= jitsuyo::assign_val(localization_section, "num_particles", robot->num_particles); valid_section &= jitsuyo::assign_val(localization_section, "min_centered_particles_ratio", robot->min_centered_particles_ratio); valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", robot->short_term_avg_ratio); From ef150af477a31ebb27c05b9ff328ec182be0c6ea Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Fri, 9 May 2025 22:42:00 +0700 Subject: [PATCH 43/45] feat: adjust constraint and coef for localization --- src/suiryoku/locomotion/model/robot.cpp | 62 ++++++++++++++++--------- 1 file changed, 39 insertions(+), 23 deletions(-) diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index eb95185..109eeed 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -68,10 +68,10 @@ void Robot::localize() } bool evaluate_particles = num_projected_objects > 0; - evaluate_particles &= fabs(orientation_roll.degree()) <= 25; - evaluate_particles &= fabs(orientation_pitch.degree()) <= 25; - evaluate_particles &= fabs(get_pan().degree()) <= 80; - evaluate_particles &= get_tilt().degree() > -45; + evaluate_particles &= fabs(orientation_roll.degree()) <= 30; + evaluate_particles &= fabs(orientation_pitch.degree()) <= 30; + evaluate_particles &= fabs(get_pan().degree()) <= 90; + evaluate_particles &= get_tilt().degree() > -50; if (evaluate_particles) { weight_avg = 0.0; @@ -183,14 +183,14 @@ void Robot::resample_particles() int interval_y[2] = {10, 590}; bool resample_nearby = num_projected_objects == 1; - resample_nearby |= fabs(get_pan().degree()) > 50; + resample_nearby |= fabs(get_pan().degree()) > 80; resample_nearby |= get_tilt().degree() < -30; if (resample_nearby) { - interval_x[0] = position.x - 75; - interval_x[1] = position.x + 75; - interval_y[0] = position.y - 75; - interval_y[1] = position.y + 75; + interval_x[0] = position.x - 50; + interval_x[1] = position.x + 50; + interval_y[0] = position.y - 50; + interval_y[1] = position.y + 50; prob = std::min(0.05, prob); } @@ -244,18 +244,31 @@ void Robot::resample_particles() void Robot::update_motion() { static std::normal_distribution xgen(0.0, xvar), ygen(0.0, yvar); + bool update_with_noise = num_projected_objects > 0; + update_with_noise &= fabs(orientation_roll.degree()) <= 30; + update_with_noise &= fabs(orientation_pitch.degree()) <= 30; + update_with_noise &= fabs(get_pan().degree()) <= 90; + update_with_noise &= get_tilt().degree() > -50; - for (auto & p : particles) { - double static_noise_x = xgen(rand_gen) / 5.0; - double static_noise_y = ygen(rand_gen) / 5.0; - double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; - double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; - double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; - double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0; - - p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; - p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; - p.orientation = orientation; + if (update_with_noise) { + for (auto & p : particles) { + double static_noise_x = xgen(rand_gen) / 5.0; + double static_noise_y = ygen(rand_gen) / 5.0; + double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; + double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; + double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; + double y_xterm = fabs(delta_position.x) * ygen(rand_gen) / 30.0; + + p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; + p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; + p.orientation = orientation; + } + } else { + for (auto & p : particles) { + p.position.x += delta_position.x; + p.position.y += delta_position.y; + p.orientation = orientation; + } } estimated_position += delta_position; @@ -324,7 +337,7 @@ keisan::Matrix<6, 6> Robot::calculate_cost_matrix( } double Robot::calculate_total_likelihood(const Particle & particle) { - double total_likelihood = 0.0; + double total_likelihood = 1.0; std::vector landmark_groups = { {&projected_X, &field.landmarks_X}, @@ -344,7 +357,7 @@ double Robot::calculate_total_likelihood(const Particle & particle) { for (int i = 0; i < group.projected->size(); ++i) { for (int j = 0; j < group.landmarks->size(); ++j) { if (result[i][j] == 1) { - total_likelihood += calculate_object_likelihood((*group.projected)[i], (*group.landmarks)[j], particle); + total_likelihood *= calculate_object_likelihood((*group.projected)[i], (*group.landmarks)[j], particle); } } } @@ -365,12 +378,15 @@ double Robot::calculate_object_likelihood( double relative_position_x = particle.position.x + x_rot; double relative_position_y = particle.position.y + y_rot; + double dist = sqrt(dx * dx + dy * dy); + double dist_factor = keisan::map(dist, 150.0, 500.0, 1.0, 0.9); + double exponent = -0.5 * (pow((landmark.x - relative_position_x), 2) / pow(sigma_x, 2) + pow((landmark.y - relative_position_y), 2) / pow(sigma_y, 2)); - return exp(exponent) / (2 * M_PI * sigma_x * sigma_y); + return exp(exponent) / (2 * M_PI * sigma_x * sigma_y) * dist_factor; } void Robot::estimate_position() { From ffbc11983e5913f07f70460fe94e5cd47f4c8fef Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Fri, 9 May 2025 22:42:51 +0700 Subject: [PATCH 44/45] feat: locomotion follow method for continuous scanning --- .../locomotion/process/locomotion.hpp | 3 + .../locomotion/process/locomotion.cpp | 85 +++++++++++++++++++ 2 files changed, 88 insertions(+) diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp index 49b55a9..072e70e 100755 --- a/include/suiryoku/locomotion/process/locomotion.hpp +++ b/include/suiryoku/locomotion/process/locomotion.hpp @@ -47,12 +47,15 @@ class Locomotion void move_forward(const keisan::Angle & direction); bool move_forward_to(const keisan::Point2 & target); + bool move_forward_by_distance(keisan::Point2 & delta_target); bool rotate_to_target(const keisan::Angle & direction); bool rotate_to(const keisan::Angle & direction, bool a_move_only); bool move_follow_head(); bool move_follow_head(const keisan::Angle & min_tilt); + bool move_follow_head(const keisan::Angle & pan, const keisan::Angle & tilt); + bool move_follow_head(const keisan::Angle & pan, const keisan::Angle & tilt, const keisan::Angle & min_tilt); bool move_skew(const keisan::Angle & direction); bool move_skew(const keisan::Angle & direction, bool skew_left); diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp index 502dbec..a01b07d 100755 --- a/src/suiryoku/locomotion/process/locomotion.cpp +++ b/src/suiryoku/locomotion/process/locomotion.cpp @@ -465,6 +465,45 @@ bool Locomotion::move_forward_to(const keisan::Point2 & target) return false; } +bool Locomotion::move_forward_by_distance(keisan::Point2 & delta_target) +{ + double delta_x = delta_target.x; + double delta_y = delta_target.y; + + double target_distance = std::hypot(delta_x, delta_y); + + if (target_distance < 8.0) { + return true; + } + + auto direction = keisan::signed_arctan(delta_y, delta_x).normalize(); + double delta_direction = (direction - robot->orientation).normalize().degree(); + + double x_speed = keisan::map(std::abs(delta_direction), 0.0, 15.0, move_max_x, move_min_x); + if (target_distance < 100.0) { + x_speed = keisan::map(target_distance, 0.0, 100.0, move_max_x * 0.25, move_max_x); + } + + double a_speed = keisan::map(delta_direction, -25.0, 25.0, move_max_a, -move_max_a); + if (std::abs(delta_direction) > 15.0) { + a_speed = (delta_direction < 0.0) ? move_max_a : -move_max_a; + x_speed = keisan::map(std::abs(a_speed), 0.0, move_max_a, move_max_x, 0.0); + } + + x_speed = keisan::smooth(robot->x_speed, x_speed, 0.4); + + robot->x_speed = x_speed; + robot->y_speed = 0.0; + robot->a_speed = a_speed; + robot->aim_on = false; + start(); + + delta_target.x -= robot->delta_position.x; + delta_target.y -= robot->delta_position.y; + + return false; +} + bool Locomotion::rotate_to_target(const keisan::Angle & direction) { auto delta_direction = (direction - robot->orientation).normalize().degree(); @@ -515,6 +554,11 @@ bool Locomotion::move_follow_head() return move_follow_head(follow_min_tilt); } +bool Locomotion::move_follow_head(const keisan::Angle & pan, const keisan::Angle & tilt) +{ + return move_follow_head(pan, tilt, follow_min_tilt); +} + bool Locomotion::move_follow_head(const keisan::Angle & min_tilt) { double a_speed = 0.0; @@ -556,6 +600,47 @@ bool Locomotion::move_follow_head(const keisan::Angle & min_tilt) return robot->get_tilt() + robot->tilt_center < min_tilt; } +bool Locomotion::move_follow_head(const keisan::Angle & pan, const keisan::Angle & tilt, const keisan::Angle & min_tilt) +{ + double a_speed = 0.0; + if (pan.degree() < 0.0) { + a_speed = keisan::map(pan.degree(), -30.0, follow_pan_ratio * right_kick_target_pan.degree(), -follow_max_a, 0.0); + } else { + a_speed = keisan::map(pan.degree(), follow_pan_ratio * left_kick_target_pan.degree(), 30.0, 0.0, follow_max_a); + } + + double x_speed = 0.0; + if (follow_max_a != 0) { + x_speed = keisan::map(std::abs(a_speed), 0.0, follow_max_a, follow_max_x, 0.0); + x_speed = keisan::map((tilt + robot->tilt_center - min_tilt).degree(), 10.0, 0.0, x_speed, follow_min_x); + } else { + x_speed = keisan::map((tilt + robot->tilt_center - min_tilt).degree(), 10.0, 0.0, follow_max_x, follow_min_x); + const auto max_a_speed = (pan.degree() > 3.0) ? follow_l_a_offset : follow_r_a_offset; + a_speed = keisan::map(x_speed, follow_min_x, follow_max_x, 0.0, max_a_speed); + } + + double y_speed = 0.0; + if (follow_y_move){ + if (pan.degree() < -3.0) { + y_speed = keisan::map(pan.degree(), -15.0, 0.0, follow_max_ry, follow_min_ry); + } else if (pan.degree() > 3.0) { + y_speed = keisan::map(pan.degree(), 0.0, 15.0, follow_min_ly, follow_max_ly); + } + } + + double smooth_ratio = 1.0; + + x_speed = keisan::smooth(robot->x_speed, x_speed, smooth_ratio); + + robot->x_speed = x_speed; + robot->y_speed = y_speed; + robot->a_speed = a_speed; + robot->aim_on = false; + start(); + + return tilt + robot->tilt_center < min_tilt; +} + bool Locomotion::move_skew(const keisan::Angle & direction) { auto current_direction = (robot->orientation - robot->pan).normalize(); From 2537233d38e859cf931c73c7e926b6b785b5b8db Mon Sep 17 00:00:00 2001 From: mbsaloka Date: Fri, 16 May 2025 20:56:40 +0700 Subject: [PATCH 45/45] feat: add jump rejection --- include/suiryoku/locomotion/model/robot.hpp | 5 +- src/suiryoku/locomotion/model/robot.cpp | 90 ++++++++++++++------- 2 files changed, 67 insertions(+), 28 deletions(-) diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp index 6e765a3..e5fecfb 100644 --- a/include/suiryoku/locomotion/model/robot.hpp +++ b/include/suiryoku/locomotion/model/robot.hpp @@ -70,7 +70,7 @@ class Robot void print_estimate_position(); void set_initial_localization(bool initial) { initial_localization = initial; } - double calculate_landmark_cost(const keisan::Point2 & landmark, const keisan::Point2 & projected_object); + double calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2); double calculate_total_likelihood(const Particle & particle); double calculate_object_likelihood(const ProjectedObject & measurement, const keisan::Point2 & landmark, @@ -148,6 +148,9 @@ class Robot bool initial_localization; bool reset_particles; + + int pending_accept_count; + keisan::Point2 pending_estimated_position; }; } // namespace suiryoku diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp index 109eeed..cd11e43 100644 --- a/src/suiryoku/locomotion/model/robot.cpp +++ b/src/suiryoku/locomotion/model/robot.cpp @@ -40,8 +40,8 @@ Robot::Robot() short_term_avg_ratio(0.1), long_term_avg_ratio(0.001), sigma_x(5.0), sigma_y(5.0), xvar(10.0), yvar(10.0), best_particle(nullptr), rand_gen(std::random_device{}()), weight_avg(0.0), short_term_avg(0.0), long_term_avg(0.0), last_weight_avg(0.0), - estimated_position(0.0, 0.0), initial_localization(true), reset_particles(false), - num_projected_objects(0), max_object_distance(300.0, 300.0) + estimated_position(-1.0, -1.0), initial_localization(true), reset_particles(false), + num_projected_objects(0), max_object_distance(300.0, 300.0), pending_accept_count(0) { } @@ -79,11 +79,9 @@ void Robot::localize() if (weight_avg <= 0 || std::isnan(weight_avg)) { weight_avg = last_weight_avg; } else { - if (initial_localization || reset_particles) { + if (initial_localization) { short_term_avg = weight_avg; long_term_avg = weight_avg; - initial_localization = false; - reset_particles = false; } last_weight_avg = weight_avg; } @@ -106,7 +104,7 @@ void Robot::reset_localization() { particles.clear(); center_particles.clear(); clear_projected_objects(); - estimated_position = keisan::Point2(0.0, 0.0); + estimated_position = keisan::Point2(-1.0, -1.0); best_particle = nullptr; weight_avg = 0.0; short_term_avg = 0.0; @@ -114,6 +112,8 @@ void Robot::reset_localization() { last_weight_avg = 0.0; reset_particles = false; prob = 0.0; + pending_accept_count = 0; + pending_estimated_position = keisan::Point2(-1.0, -1.0); if (use_localization) { localize(); @@ -134,6 +134,8 @@ void Robot::init_particles(const keisan::Point2 init_position) double uniform_weight = 1.0 / num_particles; particles.clear(); particles.resize(num_particles); + initial_localization = true; + estimated_position = init_position; if (init_position.x == -1 && init_position.y == -1) { std::uniform_int_distribution xrg(0, 900); @@ -145,8 +147,8 @@ void Robot::init_particles(const keisan::Point2 init_position) particles[i].weight = uniform_weight; } } else { - std::normal_distribution xrg(init_position.x, 25); - std::normal_distribution yrg(init_position.y, 25); + std::normal_distribution xrg(init_position.x, 50); + std::normal_distribution yrg(init_position.y, 50); for (int i = 0; i < num_particles; ++i) { particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); @@ -166,7 +168,7 @@ void Robot::resample_particles() double max_weight = 0.0; double sum_weight = get_sum_weight(); prob = std::max(0.0, 1.0 - short_term_avg / long_term_avg); - reset_particles = prob > reset_particles_threshold; + // reset_particles = prob > reset_particles_threshold; // find the best particle if (sum_weight > 0) { @@ -184,13 +186,15 @@ void Robot::resample_particles() bool resample_nearby = num_projected_objects == 1; resample_nearby |= fabs(get_pan().degree()) > 80; - resample_nearby |= get_tilt().degree() < -30; + resample_nearby |= get_tilt().degree() < -40; + resample_nearby |= reset_particles; if (resample_nearby) { interval_x[0] = position.x - 50; interval_x[1] = position.x + 50; interval_y[0] = position.y - 50; interval_y[1] = position.y + 50; + reset_particles = false; prob = std::min(0.05, prob); } @@ -201,7 +205,7 @@ void Robot::resample_particles() std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); // adaptive noise - double max_sigma = 5.0; + double max_sigma = 3.0; double min_sigma = 0.1; double sigma_factor = 1.0 - prob; @@ -250,10 +254,10 @@ void Robot::update_motion() update_with_noise &= fabs(get_pan().degree()) <= 90; update_with_noise &= get_tilt().degree() > -50; - if (update_with_noise) { + if (update_with_noise || true) { for (auto & p : particles) { - double static_noise_x = xgen(rand_gen) / 5.0; - double static_noise_y = ygen(rand_gen) / 5.0; + double static_noise_x = xgen(rand_gen) / 10.0; + double static_noise_y = ygen(rand_gen) / 10.0; double dynamic_noise_x = fabs(delta_position.x) * xgen(rand_gen) / 5.0; double dynamic_noise_y = fabs(delta_position.y) * ygen(rand_gen) / 5.0; double x_yterm = fabs(delta_position.y) * xgen(rand_gen) / 30.0; @@ -262,12 +266,18 @@ void Robot::update_motion() p.position.x += delta_position.x + static_noise_x + dynamic_noise_x + x_yterm; p.position.y += delta_position.y + static_noise_y + dynamic_noise_y + y_xterm; p.orientation = orientation; + + p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); + p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); } } else { for (auto & p : particles) { p.position.x += delta_position.x; p.position.y += delta_position.y; p.orientation = orientation; + + p.position.x = keisan::clamp(p.position.x, 0.0, 900.0); + p.position.y = keisan::clamp(p.position.y, 0.0, 600.0); } } @@ -302,9 +312,9 @@ void Robot::calculate_weight() } } -double Robot::calculate_landmark_cost(const keisan::Point2 & landmark, const keisan::Point2 & projected_object) { - double dx = landmark.x - projected_object.x; - double dy = landmark.y - projected_object.y; +double Robot::calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2) { + double dx = point_1.x - point_2.x; + double dy = point_1.y - point_2.y; return sqrt(dx * dx + dy * dy); } @@ -329,7 +339,7 @@ keisan::Matrix<6, 6> Robot::calculate_cost_matrix( keisan::Point2 projected_object(relative_position_x, relative_position_y); - cost_matrix[i][j] = calculate_landmark_cost(landmarks[j], projected_object); + cost_matrix[i][j] = calculate_distance(landmarks[j], projected_object); } } @@ -400,8 +410,7 @@ void Robot::estimate_position() { // Find numbers of particles closed to the best particle for (int i = 0; i < num_particles; ++i) { - double distance = sqrt(pow(particles[i].position.x - best_particle->position.x, 2) + - pow(particles[i].position.y - best_particle->position.y, 2)); + double distance = calculate_distance(particles[i].position, best_particle->position); if (distance < 25.0) { centered_particles++; @@ -414,14 +423,40 @@ void Robot::estimate_position() { // Use mean of centered particles position if more than threshold are centered if (centered_particles > min_centered_particles_ratio * num_particles) { + auto last_estimated_position = estimated_position; estimated_position.x = sum_position.x / centered_particles; estimated_position.y = sum_position.y / centered_particles; estimated_position.x = keisan::clamp(estimated_position.x, 0.0, 900.0); estimated_position.y = keisan::clamp(estimated_position.y, 0.0, 600.0); - position = estimated_position; - apply_localization = true; + double jump_distance = calculate_distance(estimated_position, position); + + if (jump_distance <= 100.0 || initial_localization) { + position = estimated_position; + apply_localization = true; + if (initial_localization) { + initial_localization = false; + } + } else { + double delta_estimate = calculate_distance(pending_estimated_position, estimated_position); + if (delta_estimate < 20.0) { + pending_accept_count++; + } else { + pending_accept_count = 0; + } + pending_estimated_position = estimated_position; + + if (pending_accept_count >= 10) { + position = estimated_position; + apply_localization = true; + pending_accept_count = 0; + } else { + estimated_position = last_estimated_position; + apply_localization = false; + reset_particles = true; + } + } } } @@ -436,11 +471,11 @@ void Robot::print_particles() { reset_particles_threshold, (prob > reset_particles_threshold) ? "true" : "false"); printf("Reset particles: %s\n", reset_particles ? "true" : "false"); - printf("Short term avg: %.5f\n", short_term_avg); - printf("Long term avg: %.5f\n", long_term_avg); - printf("Last weight avg: %.5f\n", last_weight_avg); - printf("Weight avg: %.5f\n", weight_avg); - printf("Sum weights: %.5f\n", get_sum_weight()); + printf("Short term avg: %.8f\n", short_term_avg); + printf("Long term avg: %.8f\n", long_term_avg); + printf("Last weight avg: %.8f\n", last_weight_avg); + printf("Weight avg: %.8f\n", weight_avg); + printf("Sum weights: %.8f\n", get_sum_weight()); printf("========================================\n"); printf("Minimal Centered Particles: %.0f\n", min_centered_particles_ratio * num_particles); @@ -472,6 +507,7 @@ void Robot::print_particles() { printf("Num objects: %d\n", num_projected_objects); printf("Num particles: %d\n", num_particles); printf("Projected objects: %d\n", num_projected_objects); + printf("Initial Localization %d\n", initial_localization); print_estimate_position(); }