From 12d583b92ff94f492ad56646df69c7438d5ce55d Mon Sep 17 00:00:00 2001 From: enaix Date: Sun, 28 May 2023 15:50:28 +0300 Subject: [PATCH 01/20] Add pure pursuit node (WIP) --- packages/pure_pursuit/CMakeLists.txt | 41 ++++++++++++++++ .../include/pure_pursuit/pursuit.h | 48 +++++++++++++++++++ packages/pure_pursuit/package.xml | 20 ++++++++ packages/pure_pursuit/src/main.cpp | 11 +++++ packages/pure_pursuit/src/pursuit.cpp | 42 ++++++++++++++++ 5 files changed, 162 insertions(+) create mode 100644 packages/pure_pursuit/CMakeLists.txt create mode 100644 packages/pure_pursuit/include/pure_pursuit/pursuit.h create mode 100644 packages/pure_pursuit/package.xml create mode 100644 packages/pure_pursuit/src/main.cpp create mode 100644 packages/pure_pursuit/src/pursuit.cpp diff --git a/packages/pure_pursuit/CMakeLists.txt b/packages/pure_pursuit/CMakeLists.txt new file mode 100644 index 0000000..0f4e560 --- /dev/null +++ b/packages/pure_pursuit/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.8) +project(pure_pursuit) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(wbb_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(pursuit src/main.cpp src/pursuit.cpp) +target_include_directories(pursuit PUBLIC + $ + $) +target_compile_features(pursuit PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +ament_target_dependencies( + rclcpp + wbb_msgs ) + + +install(TARGETS pursuit + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h new file mode 100644 index 0000000..1199337 --- /dev/null +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -0,0 +1,48 @@ +#pragma once + +#include + +#include +#include +#include +#include + +namespace wbb +{ + +class PurePursuit : public rclcpp::Node +{ + public: + PurePursuit() : Node("pure_pursuit"); + private: + void handleTrajectory(wbb_msgs::msg::ImagePath::SharedPtr trajectory); + + void handleBotPose(wbb_msgs::msg::ImagePos::SharedPtr bot_pose); + + void sendControlCommand(); + + struct Slols + { + rclcpp::Subscription::SharedPtr trajectory = nullptr; + rclcpp::Subscription::SharedPtr bot_pose = nullptr; + } slot_; + struct Signals + { + rclcpp::Publisher::SharedPtr control = nullptr; + } signal_; + + struct State + { + wbb_msgs::msg::ImagePath::SharedPtr trajectory = nullptr; + wbb_msgs::msg::ImagePose::SharedPtr bot_pose = nullptr; + } state_; + + std::chrono::duration timeout_{0.20}; + rclcpp::TimerBase::SharedPtr timer_ = nullptr; +}; + +} // namespace wbb + + + + diff --git a/packages/pure_pursuit/package.xml b/packages/pure_pursuit/package.xml new file mode 100644 index 0000000..8a22caa --- /dev/null +++ b/packages/pure_pursuit/package.xml @@ -0,0 +1,20 @@ + + + + pure_pursuit + 0.0.0 + Simple whiteboard bot trajectory pursuit + enaix + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + rclcpp + wbb_msgs + + + ament_cmake + + diff --git a/packages/pure_pursuit/src/main.cpp b/packages/pure_pursuit/src/main.cpp new file mode 100644 index 0000000..1374c84 --- /dev/null +++ b/packages/pure_pursuit/src/main.cpp @@ -0,0 +1,11 @@ +#include + +#include "pure_pursuit/pursuit.h" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp new file mode 100644 index 0000000..3a7e531 --- /dev/null +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -0,0 +1,42 @@ +#include "pure_pursuit/pursuit.h" + +namespace wbb +{ +PurePursuit::PurePursuit() +{ + const auto qos = static_cast( + this->declare_parameter("qos", RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT)); + + std::chrono::duration period = this->declare_parameter("period", 0.02); + double lookahead = this->declare_parameter("lookahead", 0.1); + + timer_ = this->create_wall_timer(period, std::bind(&PurePursuit::sendControlCommand, this)); + + slot_.trajectory = this->create_subscription( + "/robot/motion", 1, std::bind(&PurePursuit::handleTrajectory, this, _1) + ); + + slot_.bot_pose = this->create_subscription( + "/board/image/ego", rclcpp::QoS(1).reliability(qos), + std::bind(&PurePursuit::handleBotPose, this, _1) + ); + + signal_.control = this->create_publisher("/movement", 1); +} + +void PurePursuit::handleTrajectory(wbb_msgs::msg::ImagePath::SharedPtr trajectory) +{ + state_.trajectory = std::move(trajectory); +} + +void PurePursuit::handleBotPose(wbb_msgs::msg::ImagePose::SharedPtr bot_pose) +{ + state_.bot_pose = std::move(bot_pose); +} + +void PurePursuit::sendControlCommand() +{ + // ... +} + +} // namespace wbb \ No newline at end of file From de2d741aac806e4ace294db601db459da0f7cd5a Mon Sep 17 00:00:00 2001 From: enaix Date: Mon, 29 May 2023 19:51:50 +0300 Subject: [PATCH 02/20] Add pure pursuit implementation (WIP) --- .../include/pure_pursuit/pursuit.h | 17 +++ packages/pure_pursuit/src/pursuit.cpp | 125 +++++++++++++++++- 2 files changed, 141 insertions(+), 1 deletion(-) diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h index 1199337..e86ce24 100644 --- a/packages/pure_pursuit/include/pure_pursuit/pursuit.h +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -19,6 +20,22 @@ class PurePursuit : public rclcpp::Node void handleBotPose(wbb_msgs::msg::ImagePos::SharedPtr bot_pose); + double calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first, + wbb_msgs::msg::ImagePos::SharedPtr second); + + double calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, + wbb_msgs::msg::ImagePos::SharedPtr bot_pose); + + wbb_msgs::msg::ImagePoint::SharedPtr findClosest(wbb_msgs::msg::ImagePath::SharedPtr trajectory, + wbb_msgs::msg::ImagePos::SharedPtr bot_pose); + + wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint::SharedPtr start, + wbb_msgs::msg::ImagePoint::SharedPtr end, + wbb_msgs::msg::ImagePos::SharedPtr bot_pose); + + wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, + wbb_msgs::msg::ImagePos::SharedPtr bot_pose); + void sendControlCommand(); struct Slols diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index 3a7e531..444bdfd 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -2,6 +2,7 @@ namespace wbb { + PurePursuit::PurePursuit() { const auto qos = static_cast( @@ -34,9 +35,131 @@ void PurePursuit::handleBotPose(wbb_msgs::msg::ImagePose::SharedPtr bot_pose) state_.bot_pose = std::move(bot_pose); } +double calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first, + wbb_msgs::msg::ImagePos::SharedPtr second) +{ + return std::sqrt(std::pow(first->x - second->x, 2) + + std::pow(first->y - second->y, 2)); +} + +double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, + wbb_msgs::msg::ImagePos::SharedPtr bot_pose) +{ + double chord = calculateDistance(lookahead, bot_pose); + + if (chord == 0) + return 0; + + double alpha = std::atan2(lookahead->y - bot_pose->y, lookahead->x - bot_pose->x) - + M_PI / 2 + bot_pose->theta; + + return (2 * std::sin(alpha)) / chord; +} + + + +wbb_msgs::msg::ImagePoint::SharedPtr findClosest(wbb_msgs::msg::ImagePath::SharedPtr trajectory, + wbb_msgs::msg::ImagePos::SharedPtr bot_pose) +{ + double min_dist = calculateDistance(trajectory[0], bot_pose); + wbb_msgs::msg::ImagePoint::SharedPtr min_point = trajectory[0]; + + for (auto point : trajectory->points) + { + double distance = calculateDistance(point, bot_pose); + if (distance < min_dist) + { + min_dist = distance; + min_point = point; + } + } + + return min_point; +} + +wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint::SharedPtr start, + wbb_msgs::msg::ImagePoint::SharedPtr end, + wbb_msgs::msg::ImagePos::SharedPtr bot_pose) +{ + double vector_dot_a = std::pow(end->x - start->x, 2) + std::pow(end->y - start->y, 2); + + double vector_dot_b = 2 * (start->x - bot_pose->x) * (end->x - start->x) + + 2 * (start->y - bot_pose->y) * (end->y - start->y); + + double vector_dot_c = std::pow(start->x - bot_pose->x, 2) + + std::pow(start->y - bot_pose->y, 2) - std::pow(self.lookahead, 2); + + double discr = std::pow(vector_dot_b, 2) - 4 * vector_dot_a * vector_dot_c; + + if (discr < 0 || vector_dot_a == 0) + return nullptr; + + discr = std::sqrt(discr); + + double t1 = (-vector_dot_b - discr) / (2 * vector_dot_a); + double t2 = (-vector_dot_b + discr) / (2 * vector_dot_a); + + if (t1 >= 0 && t1 <= 1) + { + wbb_msgs::msg::ImagePoint pt; + pt->x = start->x + t1 * (end->x - start->x); + pt->y = start->y + t1 * (end->y - start->y); + return pt; + } + if (t2 >= 0 && t2 <= 1) + { + wbb_msgs::msg::ImagePoint pt; + pt->x = start->x + t2 * (end->x - start->x); + pt->y = start->y + t2 * (end->y - start->y); + return pt; + } + + return nullptr; +} + +wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, + wbb_msgs::msg::ImagePos::SharedPtr bot_pose) +{ + for (size_t i = 1; i < trajectory.size(); i++) + { + wbb_msgs::msg::ImagePoint::SharedPtr pt = checkSegment(trajectory[i - 1], trajectory[i], bot_pose); + if (!pt) + continue; + + return pt; + } + + return nullptr; +} + void PurePursuit::sendControlCommand() { - // ... + auto stop = [this]() + { + wbb_msgs::msg::Control msg; + msg->curvature = 0; + msg->velocity = 0; + signal_.control->publish(msg); + }; + + if (!(state_.trajectory && state_.bot_pose)) + { + stop(); + return; + } + + wbb_msgs::msg::ImagePoint::SharedPtr lh = findLookahead(state_.trajectory, state_.bot_pose); + + if (!lh) + { + stop(); + return; + } + + wbb_msgs::msg::Control msg; + msg->curvature = calculateCurvature(lh, state_.bot_pose); + msg->velocity = 1.0; + signal_.control->publish(msg); } } // namespace wbb \ No newline at end of file From 37e922bf633e16ea2268a1e72252fd5b65c4d837 Mon Sep 17 00:00:00 2001 From: enaix Date: Wed, 31 May 2023 13:20:33 +0300 Subject: [PATCH 03/20] Fix cmakelists for pure_pursuit --- packages/pure_pursuit/CMakeLists.txt | 9 +++++---- packages/pure_pursuit/include/pure_pursuit/pursuit.h | 2 +- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/packages/pure_pursuit/CMakeLists.txt b/packages/pure_pursuit/CMakeLists.txt index 0f4e560..6cfeff4 100644 --- a/packages/pure_pursuit/CMakeLists.txt +++ b/packages/pure_pursuit/CMakeLists.txt @@ -1,9 +1,7 @@ cmake_minimum_required(VERSION 3.8) project(pure_pursuit) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +add_compile_options(-Wall -Wextra -Wpedantic) # find dependencies find_package(ament_cmake REQUIRED) @@ -14,12 +12,15 @@ find_package(wbb_msgs REQUIRED) # find_package( REQUIRED) add_executable(pursuit src/main.cpp src/pursuit.cpp) + target_include_directories(pursuit PUBLIC $ - $) + $) + target_compile_features(pursuit PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 ament_target_dependencies( + pursuit rclcpp wbb_msgs ) diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h index e86ce24..acc6685 100644 --- a/packages/pure_pursuit/include/pure_pursuit/pursuit.h +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include #include From b2d12817af224e19eda9a34924b0ac50c4028d83 Mon Sep 17 00:00:00 2001 From: enaix Date: Wed, 31 May 2023 14:15:07 +0300 Subject: [PATCH 04/20] Fix pure_pursuit typos --- .../include/pure_pursuit/pursuit.h | 17 +++++---- packages/pure_pursuit/src/main.cpp | 2 +- packages/pure_pursuit/src/pursuit.cpp | 35 ++++++++++--------- 3 files changed, 30 insertions(+), 24 deletions(-) diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h index acc6685..4d077aa 100644 --- a/packages/pure_pursuit/include/pure_pursuit/pursuit.h +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -11,30 +12,32 @@ namespace wbb { +using namespace std::placeholders; + class PurePursuit : public rclcpp::Node { public: - PurePursuit() : Node("pure_pursuit"); + PurePursuit(); private: void handleTrajectory(wbb_msgs::msg::ImagePath::SharedPtr trajectory); - void handleBotPose(wbb_msgs::msg::ImagePos::SharedPtr bot_pose); + void handleBotPose(wbb_msgs::msg::ImagePose::SharedPtr bot_pose); double calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first, - wbb_msgs::msg::ImagePos::SharedPtr second); + wbb_msgs::msg::ImagePose::SharedPtr second); double calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, - wbb_msgs::msg::ImagePos::SharedPtr bot_pose); + wbb_msgs::msg::ImagePose::SharedPtr bot_pose); wbb_msgs::msg::ImagePoint::SharedPtr findClosest(wbb_msgs::msg::ImagePath::SharedPtr trajectory, - wbb_msgs::msg::ImagePos::SharedPtr bot_pose); + wbb_msgs::msg::ImagePose::SharedPtr bot_pose); wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint::SharedPtr start, wbb_msgs::msg::ImagePoint::SharedPtr end, - wbb_msgs::msg::ImagePos::SharedPtr bot_pose); + wbb_msgs::msg::ImagePose::SharedPtr bot_pose); wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, - wbb_msgs::msg::ImagePos::SharedPtr bot_pose); + wbb_msgs::msg::ImagePose::SharedPtr bot_pose); void sendControlCommand(); diff --git a/packages/pure_pursuit/src/main.cpp b/packages/pure_pursuit/src/main.cpp index 1374c84..68351c5 100644 --- a/packages/pure_pursuit/src/main.cpp +++ b/packages/pure_pursuit/src/main.cpp @@ -5,7 +5,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index 444bdfd..67f15d5 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -3,12 +3,13 @@ namespace wbb { -PurePursuit::PurePursuit() +PurePursuit::PurePursuit() : Node("pure_pursuit") { const auto qos = static_cast( this->declare_parameter("qos", RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT)); - std::chrono::duration period = this->declare_parameter("period", 0.02); + std::chrono::duration period = std::chrono::duration( + this->declare_parameter("period", 0.02)); double lookahead = this->declare_parameter("lookahead", 0.1); timer_ = this->create_wall_timer(period, std::bind(&PurePursuit::sendControlCommand, this)); @@ -35,15 +36,15 @@ void PurePursuit::handleBotPose(wbb_msgs::msg::ImagePose::SharedPtr bot_pose) state_.bot_pose = std::move(bot_pose); } -double calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first, - wbb_msgs::msg::ImagePos::SharedPtr second) +double calculateDistance(wbb_msgs::msg::ImagePoint first, + wbb_msgs::msg::ImagePose second) { return std::sqrt(std::pow(first->x - second->x, 2) + std::pow(first->y - second->y, 2)); } double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, - wbb_msgs::msg::ImagePos::SharedPtr bot_pose) + wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { double chord = calculateDistance(lookahead, bot_pose); @@ -53,16 +54,18 @@ double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr look double alpha = std::atan2(lookahead->y - bot_pose->y, lookahead->x - bot_pose->x) - M_PI / 2 + bot_pose->theta; - return (2 * std::sin(alpha)) / chord; + if (std::cos(alpha) > 0) + return (2 * std::abs(std::sin(alpha))) / chord; + return -(2 * std::abs(std::sin(alpha))) / chord; } wbb_msgs::msg::ImagePoint::SharedPtr findClosest(wbb_msgs::msg::ImagePath::SharedPtr trajectory, - wbb_msgs::msg::ImagePos::SharedPtr bot_pose) + wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { - double min_dist = calculateDistance(trajectory[0], bot_pose); - wbb_msgs::msg::ImagePoint::SharedPtr min_point = trajectory[0]; + double min_dist = calculateDistance(trajectory->points[0], bot_pose); + wbb_msgs::msg::ImagePoint::SharedPtr min_point = trajectory->points[0]; for (auto point : trajectory->points) { @@ -77,9 +80,9 @@ wbb_msgs::msg::ImagePoint::SharedPtr findClosest(wbb_msgs::msg::ImagePath::Share return min_point; } -wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint::SharedPtr start, - wbb_msgs::msg::ImagePoint::SharedPtr end, - wbb_msgs::msg::ImagePos::SharedPtr bot_pose) +wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint start, + wbb_msgs::msg::ImagePoint end, + wbb_msgs::msg::ImagePose bot_pose) { double vector_dot_a = std::pow(end->x - start->x, 2) + std::pow(end->y - start->y, 2); @@ -118,11 +121,11 @@ wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint::Sha } wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, - wbb_msgs::msg::ImagePos::SharedPtr bot_pose) + wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { - for (size_t i = 1; i < trajectory.size(); i++) + for (size_t i = 1; i < trajectory->points.size(); i++) { - wbb_msgs::msg::ImagePoint::SharedPtr pt = checkSegment(trajectory[i - 1], trajectory[i], bot_pose); + wbb_msgs::msg::ImagePoint pt = checkSegment(trajectory->points[i - 1], trajectory->points[i], bot_pose); if (!pt) continue; @@ -148,7 +151,7 @@ void PurePursuit::sendControlCommand() return; } - wbb_msgs::msg::ImagePoint::SharedPtr lh = findLookahead(state_.trajectory, state_.bot_pose); + wbb_msgs::msg::ImagePoint lh = findLookahead(state_.trajectory, state_.bot_pose); if (!lh) { From 27b4dc1afd36f6e9d974803f1d10e86cedb4c62d Mon Sep 17 00:00:00 2001 From: enaix Date: Wed, 31 May 2023 14:31:04 +0300 Subject: [PATCH 05/20] Fix shared_ptr pointers --- packages/pure_pursuit/src/pursuit.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index 67f15d5..3df763b 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -43,7 +43,7 @@ double calculateDistance(wbb_msgs::msg::ImagePoint first, std::pow(first->y - second->y, 2)); } -double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, +double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint lookahead, wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { double chord = calculateDistance(lookahead, bot_pose); @@ -51,7 +51,7 @@ double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr look if (chord == 0) return 0; - double alpha = std::atan2(lookahead->y - bot_pose->y, lookahead->x - bot_pose->x) - + double alpha = std::atan2(lookahead.y - bot_pose->y, lookahead.x - bot_pose->x) - M_PI / 2 + bot_pose->theta; if (std::cos(alpha) > 0) @@ -65,7 +65,7 @@ wbb_msgs::msg::ImagePoint::SharedPtr findClosest(wbb_msgs::msg::ImagePath::Share wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { double min_dist = calculateDistance(trajectory->points[0], bot_pose); - wbb_msgs::msg::ImagePoint::SharedPtr min_point = trajectory->points[0]; + wbb_msgs::msg::ImagePoint min_point = trajectory->points[0]; for (auto point : trajectory->points) { @@ -82,15 +82,15 @@ wbb_msgs::msg::ImagePoint::SharedPtr findClosest(wbb_msgs::msg::ImagePath::Share wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint start, wbb_msgs::msg::ImagePoint end, - wbb_msgs::msg::ImagePose bot_pose) + wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { - double vector_dot_a = std::pow(end->x - start->x, 2) + std::pow(end->y - start->y, 2); + double vector_dot_a = std::pow(end.x - start.x, 2) + std::pow(end.y - start.y, 2); - double vector_dot_b = 2 * (start->x - bot_pose->x) * (end->x - start->x) + - 2 * (start->y - bot_pose->y) * (end->y - start->y); + double vector_dot_b = 2 * (start.x - bot_pose.x) * (end.x - start.x) + + 2 * (start.y - bot_pose.y) * (end.y - start.y); - double vector_dot_c = std::pow(start->x - bot_pose->x, 2) + - std::pow(start->y - bot_pose->y, 2) - std::pow(self.lookahead, 2); + double vector_dot_c = std::pow(start.x - bot_pose.x, 2) + + std::pow(start.y - bot_pose.y, 2) - std::pow(self.lookahead, 2); double discr = std::pow(vector_dot_b, 2) - 4 * vector_dot_a * vector_dot_c; @@ -105,15 +105,15 @@ wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint star if (t1 >= 0 && t1 <= 1) { wbb_msgs::msg::ImagePoint pt; - pt->x = start->x + t1 * (end->x - start->x); - pt->y = start->y + t1 * (end->y - start->y); + pt.x = start.x + t1 * (end.x - start.x); + pt.y = start.y + t1 * (end.y - start.y); return pt; } if (t2 >= 0 && t2 <= 1) { wbb_msgs::msg::ImagePoint pt; - pt->x = start->x + t2 * (end->x - start->x); - pt->y = start->y + t2 * (end->y - start->y); + pt.x = start.x + t2 * (end.x - start.x); + pt.y = start.y + t2 * (end.y - start.y); return pt; } From 8de9d29b3364e513ee75a90777bb45235d571d7e Mon Sep 17 00:00:00 2001 From: enaix Date: Wed, 31 May 2023 14:56:53 +0300 Subject: [PATCH 06/20] Refactor shared pointers --- .../include/pure_pursuit/pursuit.h | 8 ++--- packages/pure_pursuit/src/pursuit.cpp | 30 +++++++++---------- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h index 4d077aa..0862da5 100644 --- a/packages/pure_pursuit/include/pure_pursuit/pursuit.h +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -29,11 +29,11 @@ class PurePursuit : public rclcpp::Node double calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, wbb_msgs::msg::ImagePose::SharedPtr bot_pose); - wbb_msgs::msg::ImagePoint::SharedPtr findClosest(wbb_msgs::msg::ImagePath::SharedPtr trajectory, - wbb_msgs::msg::ImagePose::SharedPtr bot_pose); +// wbb_msgs::msg::ImagePoint findClosest(wbb_msgs::msg::ImagePath::SharedPtr trajectory, +// wbb_msgs::msg::ImagePose::SharedPtr bot_pose); - wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint::SharedPtr start, - wbb_msgs::msg::ImagePoint::SharedPtr end, + wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint start, + wbb_msgs::msg::ImagePoint end, wbb_msgs::msg::ImagePose::SharedPtr bot_pose); wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index 3df763b..b21b079 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -36,14 +36,14 @@ void PurePursuit::handleBotPose(wbb_msgs::msg::ImagePose::SharedPtr bot_pose) state_.bot_pose = std::move(bot_pose); } -double calculateDistance(wbb_msgs::msg::ImagePoint first, - wbb_msgs::msg::ImagePose second) +double calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first, + wbb_msgs::msg::ImagePose::SharedPtr second) { return std::sqrt(std::pow(first->x - second->x, 2) + std::pow(first->y - second->y, 2)); } -double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint lookahead, +double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { double chord = calculateDistance(lookahead, bot_pose); @@ -51,7 +51,7 @@ double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint lookahead, if (chord == 0) return 0; - double alpha = std::atan2(lookahead.y - bot_pose->y, lookahead.x - bot_pose->x) - + double alpha = std::atan2(lookahead->y - bot_pose->y, lookahead->x - bot_pose->x) - M_PI / 2 + bot_pose->theta; if (std::cos(alpha) > 0) @@ -59,9 +59,7 @@ double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint lookahead, return -(2 * std::abs(std::sin(alpha))) / chord; } - - -wbb_msgs::msg::ImagePoint::SharedPtr findClosest(wbb_msgs::msg::ImagePath::SharedPtr trajectory, +/*wbb_msgs::msg::ImagePoint findClosest(wbb_msgs::msg::ImagePath::SharedPtr trajectory, wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { double min_dist = calculateDistance(trajectory->points[0], bot_pose); @@ -78,7 +76,7 @@ wbb_msgs::msg::ImagePoint::SharedPtr findClosest(wbb_msgs::msg::ImagePath::Share } return min_point; -} +}*/ wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint start, wbb_msgs::msg::ImagePoint end, @@ -86,11 +84,11 @@ wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint star { double vector_dot_a = std::pow(end.x - start.x, 2) + std::pow(end.y - start.y, 2); - double vector_dot_b = 2 * (start.x - bot_pose.x) * (end.x - start.x) + - 2 * (start.y - bot_pose.y) * (end.y - start.y); + double vector_dot_b = 2 * (start.x - bot_pose->x) * (end.x - start.x) + + 2 * (start.y - bot_pose->y) * (end.y - start.y); - double vector_dot_c = std::pow(start.x - bot_pose.x, 2) + - std::pow(start.y - bot_pose.y, 2) - std::pow(self.lookahead, 2); + double vector_dot_c = std::pow(start.x - bot_pose->x, 2) + + std::pow(start.y - bot_pose->y, 2) - std::pow(self.lookahead, 2); double discr = std::pow(vector_dot_b, 2) - 4 * vector_dot_a * vector_dot_c; @@ -107,14 +105,14 @@ wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint star wbb_msgs::msg::ImagePoint pt; pt.x = start.x + t1 * (end.x - start.x); pt.y = start.y + t1 * (end.y - start.y); - return pt; + return std::make_shared(pt); } if (t2 >= 0 && t2 <= 1) { wbb_msgs::msg::ImagePoint pt; pt.x = start.x + t2 * (end.x - start.x); pt.y = start.y + t2 * (end.y - start.y); - return pt; + return std::make_shared(pt); } return nullptr; @@ -125,7 +123,7 @@ wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::Sha { for (size_t i = 1; i < trajectory->points.size(); i++) { - wbb_msgs::msg::ImagePoint pt = checkSegment(trajectory->points[i - 1], trajectory->points[i], bot_pose); + wbb_msgs::msg::ImagePoint::SharedPtr pt = checkSegment(trajectory->points[i - 1], trajectory->points[i], bot_pose); if (!pt) continue; @@ -151,7 +149,7 @@ void PurePursuit::sendControlCommand() return; } - wbb_msgs::msg::ImagePoint lh = findLookahead(state_.trajectory, state_.bot_pose); + wbb_msgs::msg::ImagePoint::SharedPtr lh = findLookahead(state_.trajectory, state_.bot_pose); if (!lh) { From 7b1f0541779020795a3f177d21015ec8cf85f602 Mon Sep 17 00:00:00 2001 From: enaix Date: Wed, 31 May 2023 15:01:56 +0300 Subject: [PATCH 07/20] Fix typos in sendControlCommand --- packages/pure_pursuit/src/pursuit.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index b21b079..2061d87 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -88,7 +88,7 @@ wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint star 2 * (start.y - bot_pose->y) * (end.y - start.y); double vector_dot_c = std::pow(start.x - bot_pose->x, 2) + - std::pow(start.y - bot_pose->y, 2) - std::pow(self.lookahead, 2); + std::pow(start.y - bot_pose->y, 2) - std::pow(lookahead, 2); double discr = std::pow(vector_dot_b, 2) - 4 * vector_dot_a * vector_dot_c; @@ -138,8 +138,8 @@ void PurePursuit::sendControlCommand() auto stop = [this]() { wbb_msgs::msg::Control msg; - msg->curvature = 0; - msg->velocity = 0; + msg.curvature = 0; + msg.velocity = 0; signal_.control->publish(msg); }; @@ -158,8 +158,8 @@ void PurePursuit::sendControlCommand() } wbb_msgs::msg::Control msg; - msg->curvature = calculateCurvature(lh, state_.bot_pose); - msg->velocity = 1.0; + msg.curvature = calculateCurvature(lh, state_.bot_pose); + msg.velocity = 1.0; signal_.control->publish(msg); } From 033a906267f30547d354eb7e8c1cd3765061452b Mon Sep 17 00:00:00 2001 From: enaix Date: Wed, 31 May 2023 15:46:58 +0300 Subject: [PATCH 08/20] Add pursuit visualization --- packages/pure_pursuit/CMakeLists.txt | 2 + .../include/pure_pursuit/pursuit.h | 7 ++ packages/pure_pursuit/src/pursuit.cpp | 80 ++++++++++++++++++- 3 files changed, 87 insertions(+), 2 deletions(-) diff --git a/packages/pure_pursuit/CMakeLists.txt b/packages/pure_pursuit/CMakeLists.txt index 6cfeff4..888dc66 100644 --- a/packages/pure_pursuit/CMakeLists.txt +++ b/packages/pure_pursuit/CMakeLists.txt @@ -7,6 +7,7 @@ add_compile_options(-Wall -Wextra -Wpedantic) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(wbb_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) @@ -22,6 +23,7 @@ target_compile_features(pursuit PUBLIC c_std_99 cxx_std_17) # Require C99 and C ament_target_dependencies( pursuit rclcpp + visualization_msgs wbb_msgs ) diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h index 0862da5..2f69be1 100644 --- a/packages/pure_pursuit/include/pure_pursuit/pursuit.h +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -8,6 +8,7 @@ #include #include #include +#include namespace wbb { @@ -39,6 +40,10 @@ class PurePursuit : public rclcpp::Node wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, wbb_msgs::msg::ImagePose::SharedPtr bot_pose); + void visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookahead); + + void visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::SharedPtr bot_pose); + void sendControlCommand(); struct Slols @@ -49,6 +54,7 @@ class PurePursuit : public rclcpp::Node struct Signals { rclcpp::Publisher::SharedPtr control = nullptr; + rclcpp::Publisher::SharedPtr visual = nullptr; } signal_; struct State @@ -58,6 +64,7 @@ class PurePursuit : public rclcpp::Node } state_; std::chrono::duration timeout_{0.20}; + double lookahead_distance; rclcpp::TimerBase::SharedPtr timer_ = nullptr; }; diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index 2061d87..395cfc1 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -10,7 +10,7 @@ PurePursuit::PurePursuit() : Node("pure_pursuit") std::chrono::duration period = std::chrono::duration( this->declare_parameter("period", 0.02)); - double lookahead = this->declare_parameter("lookahead", 0.1); + lookahead_distance = this->declare_parameter("lookahead", 0.1); timer_ = this->create_wall_timer(period, std::bind(&PurePursuit::sendControlCommand, this)); @@ -24,6 +24,7 @@ PurePursuit::PurePursuit() : Node("pure_pursuit") ); signal_.control = this->create_publisher("/movement", 1); + signal_.visual = this->create_publisher("/board/preview/pursuit", 10); } void PurePursuit::handleTrajectory(wbb_msgs::msg::ImagePath::SharedPtr trajectory) @@ -88,7 +89,7 @@ wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint star 2 * (start.y - bot_pose->y) * (end.y - start.y); double vector_dot_c = std::pow(start.x - bot_pose->x, 2) + - std::pow(start.y - bot_pose->y, 2) - std::pow(lookahead, 2); + std::pow(start.y - bot_pose->y, 2) - std::pow(lookahead_distance, 2); double discr = std::pow(vector_dot_b, 2) - 4 * vector_dot_a * vector_dot_c; @@ -133,6 +134,78 @@ wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::Sha return nullptr; } +void visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookahead) +{ + visualization_msgs::msg::ImageMarker vis_msg; + vis_msg.type = visualization_msgs::msg::ImageMarker::CIRCLE; + vis_msg.ns = ""; + msg.action = visualization_msgs::msg::ImageMarker::ADD; + + std_msgs::msg::ColorRGBA color; + + color.r = 1.0; + color.g = 1.0; + color.b = 1.0; + color.a = 0.4; + + vis_msg.outline_color = color; + vis_msg.fill_color = color; + vis_msg.filled = 1; + vis_msg.scale = 1.0; + + geometry_msgs::msg::Point pt; + pt.x = lookahead->x; + pt.y = lookahead->y; + + vis_msg.position = pt; + vis_msg.lifetime = timeout_; + + signal_.visual->publish(vis_msg); +} + +void visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::SharedPtr bot_pose) +{ + visualization_msgs::msg::ImageMarker vis_msg; + vis_msg.type = visualization_msgs::msg::ImageMarker::CIRCLE; + vis_msg.ns = ""; + msg.action = visualization_msgs::msg::ImageMarker::ADD; + + double radius = 1000; + if (curvature != 0) + radius = 1 / curvature; + + geometry_msgs::msg::Point pt; + if (radius >= 0) + { + double angle = bot_pose->theta + M_PI / 2; + pt.x = int(radius * std::cos(angle)) + bot_pose->x; + pt.y = int(radius * std::sin(angle)) + bot_pose->y; + } + else + { + double angle = bot_pose->theta - M_PI / 2; + pt.x = bot_pose->x - int(-radius * std::cos(angle)); + pt.y = bot_pose->y - int(-radius * std::sin(angle)); + } + + vis_msg.position = pt; + + std_msgs::msg::ColorRGBA color; + + color.r = 0.0; + color.g = 1.0; + color.b = 0.0; + color.a = 0.4; + + vis_msg.outline_color = color; + vis_msg.filled = 0; + vis_msg.scale = int(radius); + + vis_msg.lifetime = timeout_; + + signal_.visual->publish(vis_msg); +} + void PurePursuit::sendControlCommand() { auto stop = [this]() @@ -157,8 +230,11 @@ void PurePursuit::sendControlCommand() return; } + visualizeLookahead(lh); + wbb_msgs::msg::Control msg; msg.curvature = calculateCurvature(lh, state_.bot_pose); + visualizeRadius(msg.curvature, state_.bot_pose); msg.velocity = 1.0; signal_.control->publish(msg); } From 9b5c31632f3f024b4d6ba27c41e2acfc692ceb17 Mon Sep 17 00:00:00 2001 From: enaix Date: Wed, 31 May 2023 15:55:42 +0300 Subject: [PATCH 09/20] Fix typos --- packages/pure_pursuit/src/pursuit.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index 395cfc1..df4a266 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -37,7 +37,7 @@ void PurePursuit::handleBotPose(wbb_msgs::msg::ImagePose::SharedPtr bot_pose) state_.bot_pose = std::move(bot_pose); } -double calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first, +double PurePursuit::calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first, wbb_msgs::msg::ImagePose::SharedPtr second) { return std::sqrt(std::pow(first->x - second->x, 2) + @@ -79,7 +79,7 @@ double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr look return min_point; }*/ -wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint start, +wbb_msgs::msg::ImagePoint::SharedPtr PurePursuit::checkSegment(wbb_msgs::msg::ImagePoint start, wbb_msgs::msg::ImagePoint end, wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { @@ -119,7 +119,7 @@ wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint star return nullptr; } -wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, +wbb_msgs::msg::ImagePoint::SharedPtr PurePursuit::findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { for (size_t i = 1; i < trajectory->points.size(); i++) @@ -134,12 +134,12 @@ wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::Sha return nullptr; } -void visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookahead) +void PurePursuit::visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookahead) { visualization_msgs::msg::ImageMarker vis_msg; vis_msg.type = visualization_msgs::msg::ImageMarker::CIRCLE; vis_msg.ns = ""; - msg.action = visualization_msgs::msg::ImageMarker::ADD; + vis_msg.action = visualization_msgs::msg::ImageMarker::ADD; std_msgs::msg::ColorRGBA color; @@ -163,12 +163,12 @@ void visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookahead) signal_.visual->publish(vis_msg); } -void visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::SharedPtr bot_pose) +void PurePursuit::visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { visualization_msgs::msg::ImageMarker vis_msg; vis_msg.type = visualization_msgs::msg::ImageMarker::CIRCLE; vis_msg.ns = ""; - msg.action = visualization_msgs::msg::ImageMarker::ADD; + vis_msg.action = visualization_msgs::msg::ImageMarker::ADD; double radius = 1000; if (curvature != 0) From e5892ac8f59f50dd2294de42cca2dbd12b399936 Mon Sep 17 00:00:00 2001 From: enaix Date: Sat, 3 Jun 2023 11:33:00 +0300 Subject: [PATCH 10/20] Add more pursuit visualizations --- .../include/pure_pursuit/pursuit.h | 2 ++ packages/pure_pursuit/src/pursuit.cpp | 33 +++++++++++++++---- 2 files changed, 29 insertions(+), 6 deletions(-) diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h index 2f69be1..dcedd09 100644 --- a/packages/pure_pursuit/include/pure_pursuit/pursuit.h +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -44,6 +44,8 @@ class PurePursuit : public rclcpp::Node void visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::SharedPtr bot_pose); + void visualizeLARadius(wbb_msgs::msg::ImagePose::SharedPtr bot_pose); + void sendControlCommand(); struct Slols diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index df4a266..81dd3ad 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -19,7 +19,7 @@ PurePursuit::PurePursuit() : Node("pure_pursuit") ); slot_.bot_pose = this->create_subscription( - "/board/image/ego", rclcpp::QoS(1).reliability(qos), + "/robot/ego", rclcpp::QoS(1).reliability(qos), std::bind(&PurePursuit::handleBotPose, this, _1) ); @@ -143,10 +143,10 @@ void PurePursuit::visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookah std_msgs::msg::ColorRGBA color; - color.r = 1.0; - color.g = 1.0; + color.r = 0.0; + color.g = 0.0; color.b = 1.0; - color.a = 0.4; + color.a = 1.0; vis_msg.outline_color = color; vis_msg.fill_color = color; @@ -158,7 +158,26 @@ void PurePursuit::visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookah pt.y = lookahead->y; vis_msg.position = pt; - vis_msg.lifetime = timeout_; + //vis_msg.lifetime = timeout_; + + signal_.visual->publish(vis_msg); +} + +void PurePursuit::visualizeLARadius(wbb_msgs::msg::ImagePose::SharedPtr bot_pose) +{ + visualization_msgs::msg::ImageMarker vis_msg; + vis_msg.type = visualization_msgs::msg::ImageMarker::CIRCLE; + vis_msg.ns = ""; + vis_msg.action = visualization_msgs::msg::ImageMarker::ADD; + + vis_msg.outline_color = color; + vis_msg.filled = 0; + vis_msg.scale = int(lookahead_distance); + + geometry_msgs::msg::Point pt; + pt.x = bot_pose->x; + pt.y = bot_pose->y; + vis_msg.position = pt; signal_.visual->publish(vis_msg); } @@ -201,7 +220,7 @@ void PurePursuit::visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::Sh vis_msg.filled = 0; vis_msg.scale = int(radius); - vis_msg.lifetime = timeout_; + //vis_msg.lifetime = timeout_; signal_.visual->publish(vis_msg); } @@ -222,6 +241,8 @@ void PurePursuit::sendControlCommand() return; } + visualizeLARadius(state_.bot_pose); + wbb_msgs::msg::ImagePoint::SharedPtr lh = findLookahead(state_.trajectory, state_.bot_pose); if (!lh) From 3a5194121360289bef66b2927bcef9ace724f50a Mon Sep 17 00:00:00 2001 From: enaix Date: Sat, 3 Jun 2023 11:40:04 +0300 Subject: [PATCH 11/20] Add LA radius color --- packages/pure_pursuit/src/pursuit.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index 81dd3ad..1611d21 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -170,6 +170,13 @@ void PurePursuit::visualizeLARadius(wbb_msgs::msg::ImagePose::SharedPtr bot_pose vis_msg.ns = ""; vis_msg.action = visualization_msgs::msg::ImageMarker::ADD; + std_msgs::msg::ColorRGBA color; + + color.r = 0.0; + color.g = 0.8; + color.b = 0.2; + color.a = 0.4; + vis_msg.outline_color = color; vis_msg.filled = 0; vis_msg.scale = int(lookahead_distance); From 5b8cfd3fb5d8b69a3757e64a51fa7cc21aabb61a Mon Sep 17 00:00:00 2001 From: enaix Date: Sat, 3 Jun 2023 11:55:28 +0300 Subject: [PATCH 12/20] Fix pursuit markers ids --- packages/pure_pursuit/src/pursuit.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index 1611d21..ac9d0a7 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -10,7 +10,7 @@ PurePursuit::PurePursuit() : Node("pure_pursuit") std::chrono::duration period = std::chrono::duration( this->declare_parameter("period", 0.02)); - lookahead_distance = this->declare_parameter("lookahead", 0.1); + lookahead_distance = this->declare_parameter("lookahead", 30); timer_ = this->create_wall_timer(period, std::bind(&PurePursuit::sendControlCommand, this)); @@ -24,7 +24,7 @@ PurePursuit::PurePursuit() : Node("pure_pursuit") ); signal_.control = this->create_publisher("/movement", 1); - signal_.visual = this->create_publisher("/board/preview/pursuit", 10); + signal_.visual = this->create_publisher("/robot/control/marker", 10); } void PurePursuit::handleTrajectory(wbb_msgs::msg::ImagePath::SharedPtr trajectory) @@ -138,7 +138,7 @@ void PurePursuit::visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookah { visualization_msgs::msg::ImageMarker vis_msg; vis_msg.type = visualization_msgs::msg::ImageMarker::CIRCLE; - vis_msg.ns = ""; + vis_msg.ns = "0"; vis_msg.action = visualization_msgs::msg::ImageMarker::ADD; std_msgs::msg::ColorRGBA color; @@ -167,15 +167,15 @@ void PurePursuit::visualizeLARadius(wbb_msgs::msg::ImagePose::SharedPtr bot_pose { visualization_msgs::msg::ImageMarker vis_msg; vis_msg.type = visualization_msgs::msg::ImageMarker::CIRCLE; - vis_msg.ns = ""; + vis_msg.ns = "1"; vis_msg.action = visualization_msgs::msg::ImageMarker::ADD; std_msgs::msg::ColorRGBA color; color.r = 0.0; - color.g = 0.8; - color.b = 0.2; - color.a = 0.4; + color.g = 0.5; + color.b = 0.5; + color.a = 1.0; vis_msg.outline_color = color; vis_msg.filled = 0; @@ -193,7 +193,7 @@ void PurePursuit::visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::Sh { visualization_msgs::msg::ImageMarker vis_msg; vis_msg.type = visualization_msgs::msg::ImageMarker::CIRCLE; - vis_msg.ns = ""; + vis_msg.ns = "2"; vis_msg.action = visualization_msgs::msg::ImageMarker::ADD; double radius = 1000; @@ -221,7 +221,7 @@ void PurePursuit::visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::Sh color.r = 0.0; color.g = 1.0; color.b = 0.0; - color.a = 0.4; + color.a = 1.0; vis_msg.outline_color = color; vis_msg.filled = 0; From 3971c9a39ef27b09328b7eb2a14c4c7a6b2ea869 Mon Sep 17 00:00:00 2001 From: enaix Date: Sun, 4 Jun 2023 14:03:31 +0300 Subject: [PATCH 13/20] Use pixel scale coefficients in pursuit --- .../include/pure_pursuit/pursuit.h | 12 ++-- packages/pure_pursuit/src/pursuit.cpp | 60 ++++++++++++------- 2 files changed, 48 insertions(+), 24 deletions(-) diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h index dcedd09..0766ffb 100644 --- a/packages/pure_pursuit/include/pure_pursuit/pursuit.h +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -24,11 +24,13 @@ class PurePursuit : public rclcpp::Node void handleBotPose(wbb_msgs::msg::ImagePose::SharedPtr bot_pose); + void handlePixelScale(wbb_msgs::msg::ImagePixelScale::SharedPtr scale); + double calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first, wbb_msgs::msg::ImagePose::SharedPtr second); double calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, - wbb_msgs::msg::ImagePose::SharedPtr bot_pose); + wbb_msgs::msg::ImagePose::SharedPtr bot_pose, double scale); // wbb_msgs::msg::ImagePoint findClosest(wbb_msgs::msg::ImagePath::SharedPtr trajectory, // wbb_msgs::msg::ImagePose::SharedPtr bot_pose); @@ -40,11 +42,11 @@ class PurePursuit : public rclcpp::Node wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, wbb_msgs::msg::ImagePose::SharedPtr bot_pose); - void visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookahead); + void visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, double scale); - void visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::SharedPtr bot_pose); + void visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::SharedPtr bot_pose, double scale); - void visualizeLARadius(wbb_msgs::msg::ImagePose::SharedPtr bot_pose); + void visualizeLARadius(wbb_msgs::msg::ImagePose::SharedPtr bot_pose, double scale); void sendControlCommand(); @@ -52,6 +54,7 @@ class PurePursuit : public rclcpp::Node { rclcpp::Subscription::SharedPtr trajectory = nullptr; rclcpp::Subscription::SharedPtr bot_pose = nullptr; + rclcpp::Subscription::SharedPtr scale = nullptr; } slot_; struct Signals { @@ -63,6 +66,7 @@ class PurePursuit : public rclcpp::Node { wbb_msgs::msg::ImagePath::SharedPtr trajectory = nullptr; wbb_msgs::msg::ImagePose::SharedPtr bot_pose = nullptr; + wbb_msgs::msg::ImagePixelScale::SharedPtr scale = nullptr; } state_; std::chrono::duration timeout_{0.20}; diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index ac9d0a7..b9d6b9c 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -23,6 +23,10 @@ PurePursuit::PurePursuit() : Node("pure_pursuit") std::bind(&PurePursuit::handleBotPose, this, _1) ); + slot_.scale = this->create_subscription( + "/board/scale", 10, std::bind(&PurePursuit::handlePixelScale, this, _1) + ); + signal_.control = this->create_publisher("/movement", 1); signal_.visual = this->create_publisher("/robot/control/marker", 10); } @@ -37,6 +41,11 @@ void PurePursuit::handleBotPose(wbb_msgs::msg::ImagePose::SharedPtr bot_pose) state_.bot_pose = std::move(bot_pose); } +void PurePursuit::handlePixelScale(wbb_msgs::msg::ImagePixelScale::SharedPtr scale) +{ + state_.scale = std::move(scale); +} + double PurePursuit::calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first, wbb_msgs::msg::ImagePose::SharedPtr second) { @@ -45,16 +54,18 @@ double PurePursuit::calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first } double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, - wbb_msgs::msg::ImagePose::SharedPtr bot_pose) + wbb_msgs::msg::ImagePose::SharedPtr bot_pose, double scale) { double chord = calculateDistance(lookahead, bot_pose); - if (chord == 0) + if (chord == 0 || scale == 0) return 0; double alpha = std::atan2(lookahead->y - bot_pose->y, lookahead->x - bot_pose->x) - M_PI / 2 + bot_pose->theta; + chord *= scale; + if (std::cos(alpha) > 0) return (2 * std::abs(std::sin(alpha))) / chord; return -(2 * std::abs(std::sin(alpha))) / chord; @@ -134,7 +145,7 @@ wbb_msgs::msg::ImagePoint::SharedPtr PurePursuit::findLookahead(wbb_msgs::msg::I return nullptr; } -void PurePursuit::visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookahead) +void PurePursuit::visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, double scale) { visualization_msgs::msg::ImageMarker vis_msg; vis_msg.type = visualization_msgs::msg::ImageMarker::CIRCLE; @@ -151,11 +162,11 @@ void PurePursuit::visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookah vis_msg.outline_color = color; vis_msg.fill_color = color; vis_msg.filled = 1; - vis_msg.scale = 1.0; + vis_msg.scale = 0.1 * scale; geometry_msgs::msg::Point pt; - pt.x = lookahead->x; - pt.y = lookahead->y; + pt.x = lookahead->x * scale; + pt.y = lookahead->y * scale; vis_msg.position = pt; //vis_msg.lifetime = timeout_; @@ -163,7 +174,7 @@ void PurePursuit::visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookah signal_.visual->publish(vis_msg); } -void PurePursuit::visualizeLARadius(wbb_msgs::msg::ImagePose::SharedPtr bot_pose) +void PurePursuit::visualizeLARadius(wbb_msgs::msg::ImagePose::SharedPtr bot_pose, double scale) { visualization_msgs::msg::ImageMarker vis_msg; vis_msg.type = visualization_msgs::msg::ImageMarker::CIRCLE; @@ -179,26 +190,26 @@ void PurePursuit::visualizeLARadius(wbb_msgs::msg::ImagePose::SharedPtr bot_pose vis_msg.outline_color = color; vis_msg.filled = 0; - vis_msg.scale = int(lookahead_distance); + vis_msg.scale = int(lookahead_distance * scale); geometry_msgs::msg::Point pt; - pt.x = bot_pose->x; - pt.y = bot_pose->y; + pt.x = bot_pose->x * scale; + pt.y = bot_pose->y * scale; vis_msg.position = pt; signal_.visual->publish(vis_msg); } -void PurePursuit::visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::SharedPtr bot_pose) +void PurePursuit::visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::SharedPtr bot_pose, double scale) { visualization_msgs::msg::ImageMarker vis_msg; vis_msg.type = visualization_msgs::msg::ImageMarker::CIRCLE; vis_msg.ns = "2"; vis_msg.action = visualization_msgs::msg::ImageMarker::ADD; - double radius = 1000; - if (curvature != 0) - radius = 1 / curvature; + double radius = 100 * scale; + if (curvature != 0 && scale != 0) + radius = 1 / (curvature * scale); geometry_msgs::msg::Point pt; if (radius >= 0) @@ -214,6 +225,9 @@ void PurePursuit::visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::Sh pt.y = bot_pose->y - int(-radius * std::sin(angle)); } + pt.x *= scale; + pt.y *= scale; + vis_msg.position = pt; std_msgs::msg::ColorRGBA color; @@ -242,15 +256,21 @@ void PurePursuit::sendControlCommand() signal_.control->publish(msg); }; - if (!(state_.trajectory && state_.bot_pose)) + if (!(state_.trajectory && state_.bot_pose && state_.scale)) { stop(); return; } - visualizeLARadius(state_.bot_pose); + wbb_msgs::msg::ImagePose bot_pose_src; + wbb_msgs::msg::ImagePose::SharedPtr bot_pose = std::make_shared(bot_pose_src); + + double scale = std::pow(std::cos(bot_pose->theta), 2) * state_.scale->x + + std::pow(std::sin(bot_pose->theta), 2) * state_.scale->y; + + visualizeLARadius(bot_pose, scale); - wbb_msgs::msg::ImagePoint::SharedPtr lh = findLookahead(state_.trajectory, state_.bot_pose); + wbb_msgs::msg::ImagePoint::SharedPtr lh = findLookahead(state_.trajectory, bot_pose); if (!lh) { @@ -258,11 +278,11 @@ void PurePursuit::sendControlCommand() return; } - visualizeLookahead(lh); + visualizeLookahead(lh, scale); wbb_msgs::msg::Control msg; - msg.curvature = calculateCurvature(lh, state_.bot_pose); - visualizeRadius(msg.curvature, state_.bot_pose); + msg.curvature = calculateCurvature(lh, bot_pose, scale); + visualizeRadius(msg.curvature, bot_pose, scale); msg.velocity = 1.0; signal_.control->publish(msg); } From cf85e5a8c7162bf7e32011c078fbb4a5bc5538f6 Mon Sep 17 00:00:00 2001 From: enaix Date: Sun, 4 Jun 2023 14:31:20 +0300 Subject: [PATCH 14/20] Add missing msg import --- packages/pure_pursuit/include/pure_pursuit/pursuit.h | 1 + 1 file changed, 1 insertion(+) diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h index 0766ffb..fee4d4e 100644 --- a/packages/pure_pursuit/include/pure_pursuit/pursuit.h +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include From d67cef226b11b1415efdbe745ee79c254222da07 Mon Sep 17 00:00:00 2001 From: enaix Date: Sun, 4 Jun 2023 14:38:14 +0300 Subject: [PATCH 15/20] Fix pursuit imagescale msg usage --- packages/pure_pursuit/src/pursuit.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index b9d6b9c..d06ada2 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -265,8 +265,8 @@ void PurePursuit::sendControlCommand() wbb_msgs::msg::ImagePose bot_pose_src; wbb_msgs::msg::ImagePose::SharedPtr bot_pose = std::make_shared(bot_pose_src); - double scale = std::pow(std::cos(bot_pose->theta), 2) * state_.scale->x + - std::pow(std::sin(bot_pose->theta), 2) * state_.scale->y; + double scale = std::pow(std::cos(bot_pose->theta), 2) * state_.scale->scale_x + + std::pow(std::sin(bot_pose->theta), 2) * state_.scale->scale_y; visualizeLARadius(bot_pose, scale); From d10f4cf9a6622d120c6f61a5f42fb9181fa6cea0 Mon Sep 17 00:00:00 2001 From: enaix Date: Mon, 5 Jun 2023 16:56:53 +0300 Subject: [PATCH 16/20] Fix pursuit scale usage --- packages/pure_pursuit/src/pursuit.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index d06ada2..1a3952e 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -24,7 +24,7 @@ PurePursuit::PurePursuit() : Node("pure_pursuit") ); slot_.scale = this->create_subscription( - "/board/scale", 10, std::bind(&PurePursuit::handlePixelScale, this, _1) + "/board/scale", 1, std::bind(&PurePursuit::handlePixelScale, this, _1) ); signal_.control = this->create_publisher("/movement", 1); @@ -64,7 +64,7 @@ double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr look double alpha = std::atan2(lookahead->y - bot_pose->y, lookahead->x - bot_pose->x) - M_PI / 2 + bot_pose->theta; - chord *= scale; + chord /= scale; if (std::cos(alpha) > 0) return (2 * std::abs(std::sin(alpha))) / chord; @@ -190,11 +190,11 @@ void PurePursuit::visualizeLARadius(wbb_msgs::msg::ImagePose::SharedPtr bot_pose vis_msg.outline_color = color; vis_msg.filled = 0; - vis_msg.scale = int(lookahead_distance * scale); + vis_msg.scale = int(lookahead_distance / scale); geometry_msgs::msg::Point pt; - pt.x = bot_pose->x * scale; - pt.y = bot_pose->y * scale; + pt.x = bot_pose->x; + pt.y = bot_pose->y; vis_msg.position = pt; signal_.visual->publish(vis_msg); @@ -207,7 +207,7 @@ void PurePursuit::visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::Sh vis_msg.ns = "2"; vis_msg.action = visualization_msgs::msg::ImageMarker::ADD; - double radius = 100 * scale; + double radius = 100 / scale; if (curvature != 0 && scale != 0) radius = 1 / (curvature * scale); @@ -225,9 +225,6 @@ void PurePursuit::visualizeRadius(double curvature, wbb_msgs::msg::ImagePose::Sh pt.y = bot_pose->y - int(-radius * std::sin(angle)); } - pt.x *= scale; - pt.y *= scale; - vis_msg.position = pt; std_msgs::msg::ColorRGBA color; From a4366b7c9dab79f1053adccb5cbe284d51da3057 Mon Sep 17 00:00:00 2001 From: enaix Date: Tue, 6 Jun 2023 12:20:10 +0300 Subject: [PATCH 17/20] Start pursuit traverse from the last point --- .../pure_pursuit/include/pure_pursuit/pursuit.h | 2 ++ packages/pure_pursuit/src/pursuit.cpp | 13 +++++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h index fee4d4e..36c6a47 100644 --- a/packages/pure_pursuit/include/pure_pursuit/pursuit.h +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -68,10 +68,12 @@ class PurePursuit : public rclcpp::Node wbb_msgs::msg::ImagePath::SharedPtr trajectory = nullptr; wbb_msgs::msg::ImagePose::SharedPtr bot_pose = nullptr; wbb_msgs::msg::ImagePixelScale::SharedPtr scale = nullptr; + wbb_msgs::msg::ImagePoint::SharedPtr last_point = nullptr; } state_; std::chrono::duration timeout_{0.20}; double lookahead_distance; + size_t trajectory_pos; rclcpp::TimerBase::SharedPtr timer_ = nullptr; }; diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index 1a3952e..d87fe5e 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -11,6 +11,7 @@ PurePursuit::PurePursuit() : Node("pure_pursuit") std::chrono::duration period = std::chrono::duration( this->declare_parameter("period", 0.02)); lookahead_distance = this->declare_parameter("lookahead", 30); + trajectory_pos = 0; timer_ = this->create_wall_timer(period, std::bind(&PurePursuit::sendControlCommand, this)); @@ -33,7 +34,11 @@ PurePursuit::PurePursuit() : Node("pure_pursuit") void PurePursuit::handleTrajectory(wbb_msgs::msg::ImagePath::SharedPtr trajectory) { - state_.trajectory = std::move(trajectory); + if (trajectory != state_.trajectory) + { + state_.trajectory = std::move(trajectory); + trajectory_pos = 0; + } } void PurePursuit::handleBotPose(wbb_msgs::msg::ImagePose::SharedPtr bot_pose) @@ -133,12 +138,16 @@ wbb_msgs::msg::ImagePoint::SharedPtr PurePursuit::checkSegment(wbb_msgs::msg::Im wbb_msgs::msg::ImagePoint::SharedPtr PurePursuit::findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, wbb_msgs::msg::ImagePose::SharedPtr bot_pose) { - for (size_t i = 1; i < trajectory->points.size(); i++) + for (size_t i = trajectory_pos + 1; i < trajectory->points.size(); i++) { wbb_msgs::msg::ImagePoint::SharedPtr pt = checkSegment(trajectory->points[i - 1], trajectory->points[i], bot_pose); if (!pt) continue; + if (state_.last_point && state_.last_point != pt) + trajectory_pos++; + + state_.last_point = pt; return pt; } From 4f668258a6674fea374f00965c0e8de2783729ee Mon Sep 17 00:00:00 2001 From: enaix Date: Wed, 7 Jun 2023 15:41:57 +0300 Subject: [PATCH 18/20] Include scale in lookahead calculation --- .../include/pure_pursuit/pursuit.h | 6 ++++-- packages/pure_pursuit/src/pursuit.cpp | 18 ++++++++++-------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h index 36c6a47..12663c3 100644 --- a/packages/pure_pursuit/include/pure_pursuit/pursuit.h +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -38,10 +38,12 @@ class PurePursuit : public rclcpp::Node wbb_msgs::msg::ImagePoint::SharedPtr checkSegment(wbb_msgs::msg::ImagePoint start, wbb_msgs::msg::ImagePoint end, - wbb_msgs::msg::ImagePose::SharedPtr bot_pose); + wbb_msgs::msg::ImagePose::SharedPtr bot_pose, + double scale); wbb_msgs::msg::ImagePoint::SharedPtr findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, - wbb_msgs::msg::ImagePose::SharedPtr bot_pose); + wbb_msgs::msg::ImagePose::SharedPtr bot_pose, + double scale); void visualizeLookahead(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, double scale); diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index d87fe5e..8bf495b 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -10,7 +10,7 @@ PurePursuit::PurePursuit() : Node("pure_pursuit") std::chrono::duration period = std::chrono::duration( this->declare_parameter("period", 0.02)); - lookahead_distance = this->declare_parameter("lookahead", 30); + lookahead_distance = this->declare_parameter("lookahead", 0.2); trajectory_pos = 0; timer_ = this->create_wall_timer(period, std::bind(&PurePursuit::sendControlCommand, this)); @@ -96,8 +96,9 @@ double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr look }*/ wbb_msgs::msg::ImagePoint::SharedPtr PurePursuit::checkSegment(wbb_msgs::msg::ImagePoint start, - wbb_msgs::msg::ImagePoint end, - wbb_msgs::msg::ImagePose::SharedPtr bot_pose) + wbb_msgs::msg::ImagePoint end, + wbb_msgs::msg::ImagePose::SharedPtr bot_pose, + double scale) { double vector_dot_a = std::pow(end.x - start.x, 2) + std::pow(end.y - start.y, 2); @@ -105,7 +106,7 @@ wbb_msgs::msg::ImagePoint::SharedPtr PurePursuit::checkSegment(wbb_msgs::msg::Im 2 * (start.y - bot_pose->y) * (end.y - start.y); double vector_dot_c = std::pow(start.x - bot_pose->x, 2) + - std::pow(start.y - bot_pose->y, 2) - std::pow(lookahead_distance, 2); + std::pow(start.y - bot_pose->y, 2) - std::pow(lookahead_distance / scale, 2); double discr = std::pow(vector_dot_b, 2) - 4 * vector_dot_a * vector_dot_c; @@ -136,11 +137,12 @@ wbb_msgs::msg::ImagePoint::SharedPtr PurePursuit::checkSegment(wbb_msgs::msg::Im } wbb_msgs::msg::ImagePoint::SharedPtr PurePursuit::findLookahead(wbb_msgs::msg::ImagePath::SharedPtr trajectory, - wbb_msgs::msg::ImagePose::SharedPtr bot_pose) + wbb_msgs::msg::ImagePose::SharedPtr bot_pose, double scale) { for (size_t i = trajectory_pos + 1; i < trajectory->points.size(); i++) { - wbb_msgs::msg::ImagePoint::SharedPtr pt = checkSegment(trajectory->points[i - 1], trajectory->points[i], bot_pose); + wbb_msgs::msg::ImagePoint::SharedPtr pt = checkSegment(trajectory->points[i - 1],trajectory->points[i], + bot_pose, scale); if (!pt) continue; @@ -268,7 +270,7 @@ void PurePursuit::sendControlCommand() return; } - wbb_msgs::msg::ImagePose bot_pose_src; + wbb_msgs::msg::ImagePose bot_pose_src = *state_.bot_pose; wbb_msgs::msg::ImagePose::SharedPtr bot_pose = std::make_shared(bot_pose_src); double scale = std::pow(std::cos(bot_pose->theta), 2) * state_.scale->scale_x + @@ -276,7 +278,7 @@ void PurePursuit::sendControlCommand() visualizeLARadius(bot_pose, scale); - wbb_msgs::msg::ImagePoint::SharedPtr lh = findLookahead(state_.trajectory, bot_pose); + wbb_msgs::msg::ImagePoint::SharedPtr lh = findLookahead(state_.trajectory, bot_pose, scale); if (!lh) { From f14e2e0dd0b35fde3b469b7092a7e198539f189c Mon Sep 17 00:00:00 2001 From: enaix Date: Thu, 8 Jun 2023 16:15:17 +0300 Subject: [PATCH 19/20] Add velocity calculation --- .../include/pure_pursuit/pursuit.h | 2 +- packages/pure_pursuit/src/pursuit.cpp | 18 +++++++++++------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/packages/pure_pursuit/include/pure_pursuit/pursuit.h b/packages/pure_pursuit/include/pure_pursuit/pursuit.h index 12663c3..2a7e571 100644 --- a/packages/pure_pursuit/include/pure_pursuit/pursuit.h +++ b/packages/pure_pursuit/include/pure_pursuit/pursuit.h @@ -30,7 +30,7 @@ class PurePursuit : public rclcpp::Node double calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first, wbb_msgs::msg::ImagePose::SharedPtr second); - double calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, + std::pair calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, wbb_msgs::msg::ImagePose::SharedPtr bot_pose, double scale); // wbb_msgs::msg::ImagePoint findClosest(wbb_msgs::msg::ImagePath::SharedPtr trajectory, diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index 8bf495b..96d3003 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -58,7 +58,7 @@ double PurePursuit::calculateDistance(wbb_msgs::msg::ImagePoint::SharedPtr first std::pow(first->y - second->y, 2)); } -double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, +std::pair PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr lookahead, wbb_msgs::msg::ImagePose::SharedPtr bot_pose, double scale) { double chord = calculateDistance(lookahead, bot_pose); @@ -70,10 +70,13 @@ double PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePoint::SharedPtr look M_PI / 2 + bot_pose->theta; chord /= scale; + double velocity = 0.5; + if (std::sin(alpha) < 0) + velocity *= -1; - if (std::cos(alpha) > 0) - return (2 * std::abs(std::sin(alpha))) / chord; - return -(2 * std::abs(std::sin(alpha))) / chord; + if (std::cos(alpha) >= 0) + return {(2 * std::abs(std::sin(alpha))) / chord, velocity}; + return {-(2 * std::abs(std::sin(alpha))) / chord, velocity}; } /*wbb_msgs::msg::ImagePoint findClosest(wbb_msgs::msg::ImagePath::SharedPtr trajectory, @@ -276,7 +279,7 @@ void PurePursuit::sendControlCommand() double scale = std::pow(std::cos(bot_pose->theta), 2) * state_.scale->scale_x + std::pow(std::sin(bot_pose->theta), 2) * state_.scale->scale_y; - visualizeLARadius(bot_pose, scale); + //visualizeLARadius(bot_pose, scale); wbb_msgs::msg::ImagePoint::SharedPtr lh = findLookahead(state_.trajectory, bot_pose, scale); @@ -289,9 +292,10 @@ void PurePursuit::sendControlCommand() visualizeLookahead(lh, scale); wbb_msgs::msg::Control msg; - msg.curvature = calculateCurvature(lh, bot_pose, scale); + std::pair prop = calculateCurvature(lh, bot_pose, scale); + msg.curvature = prop.first; visualizeRadius(msg.curvature, bot_pose, scale); - msg.velocity = 1.0; + msg.velocity = prop.second; signal_.control->publish(msg); } From 976cd561fc42b308854826e3041590575c577f8d Mon Sep 17 00:00:00 2001 From: enaix Date: Thu, 8 Jun 2023 16:36:09 +0300 Subject: [PATCH 20/20] Fix curvature scale --- packages/pure_pursuit/src/pursuit.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/packages/pure_pursuit/src/pursuit.cpp b/packages/pure_pursuit/src/pursuit.cpp index 96d3003..022fdbd 100644 --- a/packages/pure_pursuit/src/pursuit.cpp +++ b/packages/pure_pursuit/src/pursuit.cpp @@ -64,12 +64,12 @@ std::pair PurePursuit::calculateCurvature(wbb_msgs::msg::ImagePo double chord = calculateDistance(lookahead, bot_pose); if (chord == 0 || scale == 0) - return 0; + return {0.0, 0.0}; double alpha = std::atan2(lookahead->y - bot_pose->y, lookahead->x - bot_pose->x) - M_PI / 2 + bot_pose->theta; - chord /= scale; + chord *= scale; double velocity = 0.5; if (std::sin(alpha) < 0) velocity *= -1; @@ -296,6 +296,8 @@ void PurePursuit::sendControlCommand() msg.curvature = prop.first; visualizeRadius(msg.curvature, bot_pose, scale); msg.velocity = prop.second; + if (msg.curvature != 0) + RCLCPP_INFO(this->get_logger(), "%f %f", 1/msg.curvature, msg.velocity); signal_.control->publish(msg); }