From a409150ea60c3840c87303fed9f64f7c14167278 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Tue, 25 Feb 2025 19:17:42 +0100 Subject: [PATCH 1/8] Added mg_msgs as requirement to magrob --- mg_odometry/CMakeLists.txt | 1 + mg_odometry/package.xml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/mg_odometry/CMakeLists.txt b/mg_odometry/CMakeLists.txt index 48da219..464acae 100644 --- a/mg_odometry/CMakeLists.txt +++ b/mg_odometry/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rclcpp REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(mg_msgs REQUIRED) include(FindPkgConfig) pkg_search_module(LIBSERIAL REQUIRED libserial) diff --git a/mg_odometry/package.xml b/mg_odometry/package.xml index aca3299..ff8c045 100644 --- a/mg_odometry/package.xml +++ b/mg_odometry/package.xml @@ -15,6 +15,8 @@ tf2_geometry_msgs libserial-dev + mg_msgs + ament_lint_auto ament_lint_common -- 2.49.0 From fbab22835bc36ae886a703b03937cc8401faac5a Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Wed, 26 Feb 2025 02:06:50 +0100 Subject: [PATCH 2/8] Added more missing dependencies --- mg_odometry/CMakeLists.txt | 3 +++ mg_odometry/package.xml | 1 + 2 files changed, 4 insertions(+) diff --git a/mg_odometry/CMakeLists.txt b/mg_odometry/CMakeLists.txt index 464acae..fdf9446 100644 --- a/mg_odometry/CMakeLists.txt +++ b/mg_odometry/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rclcpp REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(mg_msgs REQUIRED) include(FindPkgConfig) pkg_search_module(LIBSERIAL REQUIRED libserial) @@ -24,6 +25,8 @@ add_executable(mg_odom_publisher src/mg_odom_publisher.cpp) ament_target_dependencies( mg_odom_publisher + mg_msgs + std_srvs tf2 tf2_ros tf2_geometry_msgs diff --git a/mg_odometry/package.xml b/mg_odometry/package.xml index ff8c045..562c240 100644 --- a/mg_odometry/package.xml +++ b/mg_odometry/package.xml @@ -10,6 +10,7 @@ ament_cmake rclcpp + std_srvs tf2 tf2_ros tf2_geometry_msgs -- 2.49.0 From f973dfc9d8e0ec8385ee0689adc9cd9741149d21 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Wed, 26 Feb 2025 02:07:07 +0100 Subject: [PATCH 3/8] Added SetWidth service --- mg_msgs/CMakeLists.txt | 1 + mg_msgs/srv/SetWidth.srv | 2 ++ 2 files changed, 3 insertions(+) create mode 100644 mg_msgs/srv/SetWidth.srv diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt index b0c0eb2..ff4e808 100644 --- a/mg_msgs/CMakeLists.txt +++ b/mg_msgs/CMakeLists.txt @@ -14,6 +14,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/MoveStraight.action" "action/MovePoint.action" "action/LookAt.action" + "srv/SetWidth.srv" ) ament_package() \ No newline at end of file diff --git a/mg_msgs/srv/SetWidth.srv b/mg_msgs/srv/SetWidth.srv new file mode 100644 index 0000000..7e44f9a --- /dev/null +++ b/mg_msgs/srv/SetWidth.srv @@ -0,0 +1,2 @@ +float64 width +--- \ No newline at end of file -- 2.49.0 From 6cc4ce01d4b64eaa3793a6c0c3b025c22dd567ad Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Wed, 26 Feb 2025 02:07:30 +0100 Subject: [PATCH 4/8] Added Zero and SetWidth services --- mg_odometry/src/mg_odom_publisher.cpp | 40 +++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/mg_odometry/src/mg_odom_publisher.cpp b/mg_odometry/src/mg_odom_publisher.cpp index e6504e3..bceac11 100644 --- a/mg_odometry/src/mg_odom_publisher.cpp +++ b/mg_odometry/src/mg_odom_publisher.cpp @@ -1,9 +1,15 @@ +#include #include +#include #include #include +#include #include #include +#include "mg_msgs/srv/set_width.hpp" +#include "std_srvs/srv/empty.hpp" + #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_ros/transform_broadcaster.h" @@ -18,6 +24,9 @@ constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1"; +using SetWidthSrv = mg_msgs::srv::SetWidth; +using ZeroSrv = std_srvs::srv::Empty; + class MgOdomPublisher : public rclcpp::Node { public: MgOdomPublisher() : Node("mg_odom_publisher") { @@ -29,13 +38,44 @@ class MgOdomPublisher : public rclcpp::Node { timer_ = this->create_wall_timer(std::chrono::milliseconds(TIMEOUT), std::bind(&MgOdomPublisher::timer_callback, this)); + + set_width_service_ = create_service( + "/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1)); + + zero_service_ + = create_service("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1)); } private: std::shared_ptr tf_broadcaster_; + rclcpp::Service::SharedPtr set_width_service_; + rclcpp::Service::SharedPtr zero_service_; rclcpp::TimerBase::SharedPtr timer_; LibSerial::SerialStream enc_serial_port_; + void set_width(const std::shared_ptr request) { + RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width); + + if (!enc_serial_port_.IsOpen()) { + union { + std::array bytes; + double data; + } value{}; + value.data = request->width; + + enc_serial_port_ << "w;"; + for (const auto& byte : value.bytes) { enc_serial_port_ << byte; } + } + } + + void zero(const std::shared_ptr /*request*/) { + RCLCPP_INFO(get_logger(), "Zeroing odometry"); + + if (!enc_serial_port_.IsOpen()) { + enc_serial_port_ << "z;"; + } + } + void timer_callback() { double _x = NAN; double _y = NAN; -- 2.49.0 From 5e2cc0b5af443bc5afda1181dc4e9be4c247e7b3 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Wed, 26 Feb 2025 16:26:35 +0100 Subject: [PATCH 5/8] Added rotate action --- mg_msgs/CMakeLists.txt | 1 + mg_msgs/action/Rotate.action | 25 +++++++++++++++++++++++++ 2 files changed, 26 insertions(+) create mode 100644 mg_msgs/action/Rotate.action diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt index ff4e808..9000f71 100644 --- a/mg_msgs/CMakeLists.txt +++ b/mg_msgs/CMakeLists.txt @@ -14,6 +14,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/MoveStraight.action" "action/MovePoint.action" "action/LookAt.action" + "action/Rotate.action" "srv/SetWidth.srv" ) diff --git a/mg_msgs/action/Rotate.action b/mg_msgs/action/Rotate.action new file mode 100644 index 0000000..3948fa3 --- /dev/null +++ b/mg_msgs/action/Rotate.action @@ -0,0 +1,25 @@ + +float64 angle + +float64 step 0.01 +float64 tolerance 0.001 +float64 max_wheel_speed 3 +float64 max_angular 3.14 +float64 max_vel 2 +float64 pos_mult 15 +float64 ornt_mult 4 +float64 t_ornt_mult 0.1 +float64 obst_mult_a -5 +float64 obst_mult_b -30 +float64 obst_mult_c -9 +string[] ignore_tags [] +--- +uint8 SUCCESS=0 +uint8 BLOCKED=1 +uint8 TIMEOUT=2 +uint8 MISALIGNED=3 +uint8 UNKNOWN=254 + +uint8 error +--- +float64 distance_passed \ No newline at end of file -- 2.49.0 From 43da8a3a37ef1be0cafe352bc9bd76baa8406998 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Wed, 26 Feb 2025 16:29:20 +0100 Subject: [PATCH 6/8] Added dwm rotate --- mg_navigation/include/handlers/dwm.hpp | 1 + mg_navigation/include/handlers/dwm_rotate.hpp | 66 +++++++++++++++++++ mg_navigation/include/mg_navigation.hpp | 5 +- mg_navigation/src/mg_navigation.cpp | 15 +++-- 4 files changed, 82 insertions(+), 5 deletions(-) create mode 100644 mg_navigation/include/handlers/dwm_rotate.hpp diff --git a/mg_navigation/include/handlers/dwm.hpp b/mg_navigation/include/handlers/dwm.hpp index 6788d46..642455d 100644 --- a/mg_navigation/include/handlers/dwm.hpp +++ b/mg_navigation/include/handlers/dwm.hpp @@ -4,3 +4,4 @@ #include "handlers/dwm_point.hpp" #include "handlers/dwm_forward.hpp" #include "handlers/dwm_lookat.hpp" +#include "handlers/dwm_rotate.hpp" diff --git a/mg_navigation/include/handlers/dwm_rotate.hpp b/mg_navigation/include/handlers/dwm_rotate.hpp new file mode 100644 index 0000000..44670ce --- /dev/null +++ b/mg_navigation/include/handlers/dwm_rotate.hpp @@ -0,0 +1,66 @@ +#pragma once +#include "handlers/dwm_core.hpp" + +#include "glm/gtx/norm.hpp" +#include "glm/gtx/rotate_vector.hpp" +#include "glm/gtx/vector_angle.hpp" +#include + +namespace mg { + + template <> + inline bool DWM::target_check() { + + const double a = glm::abs(theta - target_ornt); + const double b = (a > glm::pi()) ? glm::two_pi() - a : a; + + return b > goal->tolerance; + } + + template <> + inline void DWM::target_init() { + target_ornt = goal->angle; + } + + template <> + inline bool DWM::condition_check() { + return false; + } + + template <> + inline double DWM::calc_score(int vl, int vr) { + constexpr double delta = 0.5; + const auto [v, w] = to_vel(step * vl, step * vr); + const double n_theta = theta + w * delta; + + double dist_old = glm::abs(target_ornt - theta); + double dist_new = glm::abs(target_ornt - n_theta); + + dist_old = (dist_old > glm::pi()) ? glm::two_pi() - dist_old : dist_old; + dist_new = (dist_new > glm::pi()) ? glm::two_pi() - dist_new : dist_new; + + const double score = goal->ornt_mult * (dist_old - dist_new); + + return score; + } + + template <> + inline void DWM::populate_candidates(std::vector& vl, + std::vector& vr, + int dimx, + int) { + vl.clear(); + vr.clear(); + + for (int i = -dimx / 2; i <= dimx / 2; i++) { + auto [v, w] = to_vel(step * (cvl + i), step * (cvr + i)); + + if (step * abs(cvl - i) <= goal->max_wheel_speed && step * abs(cvr + i) <= goal->max_wheel_speed + && glm::abs(w) < goal->max_angular) { + vl.push_back(cvl - i); + vr.push_back(cvr + i); + } + } + } + +} \ No newline at end of file diff --git a/mg_navigation/include/mg_navigation.hpp b/mg_navigation/include/mg_navigation.hpp index c8787f7..7c92935 100644 --- a/mg_navigation/include/mg_navigation.hpp +++ b/mg_navigation/include/mg_navigation.hpp @@ -8,6 +8,7 @@ #include "mg_msgs/action/move_point.hpp" #include "mg_msgs/action/move_straight.hpp" #include "mg_msgs/action/look_at.hpp" +#include "mg_msgs/action/rotate.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -26,12 +27,14 @@ namespace mg { using MovePoint = mg_msgs::action::MovePoint; using MoveStraight = mg_msgs::action::MoveStraight; using LookAt = mg_msgs::action::LookAt; + using Rotate = mg_msgs::action::Rotate; rclcpp::Publisher::SharedPtr pub_twist; - rclcpp_action::Server::SharedPtr sv_look_at; rclcpp_action::Server::SharedPtr sv_move_point; rclcpp_action::Server::SharedPtr sv_move_straight; + rclcpp_action::Server::SharedPtr sv_look_at; + rclcpp_action::Server::SharedPtr sv_rotate; rclcpp::Subscription::SharedPtr tf2_subscription_; tf2_ros::Buffer::SharedPtr tf2_buffer; diff --git a/mg_navigation/src/mg_navigation.cpp b/mg_navigation/src/mg_navigation.cpp index c1da57b..18a2b83 100644 --- a/mg_navigation/src/mg_navigation.cpp +++ b/mg_navigation/src/mg_navigation.cpp @@ -60,6 +60,13 @@ namespace mg { std::bind(&MgNavigationServer::handle_cancel, this, _1, "MovePoint"), std::bind(&MgNavigationServer::handle_accepted, this, _1, "MovePoint")); + sv_move_straight = rclcpp_action::create_server( + this, + "MoveStraight", + std::bind(&MgNavigationServer::handle_goal, this, _1, _2, "MoveStraight"), + std::bind(&MgNavigationServer::handle_cancel, this, _1, "MoveStraight"), + std::bind(&MgNavigationServer::handle_accepted, this, _1, "MoveStraight")); + sv_look_at = rclcpp_action::create_server( this, "LookAt", @@ -67,12 +74,12 @@ namespace mg { std::bind(&MgNavigationServer::handle_cancel, this, _1, "LookAt"), std::bind(&MgNavigationServer::handle_accepted, this, _1, "LookAt")); - sv_move_straight = rclcpp_action::create_server( + sv_rotate = rclcpp_action::create_server( this, "MoveStraight", - std::bind(&MgNavigationServer::handle_goal, this, _1, _2, "MoveStraight"), - std::bind(&MgNavigationServer::handle_cancel, this, _1, "MoveStraight"), - std::bind(&MgNavigationServer::handle_accepted, this, _1, "MoveStraight")); + std::bind(&MgNavigationServer::handle_goal, this, _1, _2, "MoveStraight"), + std::bind(&MgNavigationServer::handle_cancel, this, _1, "MoveStraight"), + std::bind(&MgNavigationServer::handle_accepted, this, _1, "MoveStraight")); } }; -- 2.49.0 From 9dee466e0e6abcc9084292287158a5005fffd3dc Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Wed, 26 Feb 2025 16:47:58 +0100 Subject: [PATCH 7/8] Fixed service name confilict --- mg_navigation/src/mg_navigation.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mg_navigation/src/mg_navigation.cpp b/mg_navigation/src/mg_navigation.cpp index 18a2b83..f1fa37d 100644 --- a/mg_navigation/src/mg_navigation.cpp +++ b/mg_navigation/src/mg_navigation.cpp @@ -76,10 +76,10 @@ namespace mg { sv_rotate = rclcpp_action::create_server( this, - "MoveStraight", - std::bind(&MgNavigationServer::handle_goal, this, _1, _2, "MoveStraight"), - std::bind(&MgNavigationServer::handle_cancel, this, _1, "MoveStraight"), - std::bind(&MgNavigationServer::handle_accepted, this, _1, "MoveStraight")); + "Rotate", + std::bind(&MgNavigationServer::handle_goal, this, _1, _2, "Rotate"), + std::bind(&MgNavigationServer::handle_cancel, this, _1, "Rotate"), + std::bind(&MgNavigationServer::handle_accepted, this, _1, "Rotate")); } }; -- 2.49.0 From 4cd994cf7accc1adbb8f6cebe33815e99b12213b Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Fri, 28 Feb 2025 19:43:00 +0100 Subject: [PATCH 8/8] Fixed services for parameter adjusting not working --- mg_odometry/src/mg_odom_publisher.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/mg_odometry/src/mg_odom_publisher.cpp b/mg_odometry/src/mg_odom_publisher.cpp index bceac11..3c5d0ff 100644 --- a/mg_odometry/src/mg_odom_publisher.cpp +++ b/mg_odometry/src/mg_odom_publisher.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "mg_msgs/srv/set_width.hpp" #include "std_srvs/srv/empty.hpp" @@ -56,9 +57,9 @@ class MgOdomPublisher : public rclcpp::Node { void set_width(const std::shared_ptr request) { RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width); - if (!enc_serial_port_.IsOpen()) { + if (enc_serial_port_.IsOpen()) { union { - std::array bytes; + std::array bytes; double data; } value{}; value.data = request->width; @@ -71,7 +72,7 @@ class MgOdomPublisher : public rclcpp::Node { void zero(const std::shared_ptr /*request*/) { RCLCPP_INFO(get_logger(), "Zeroing odometry"); - if (!enc_serial_port_.IsOpen()) { + if (enc_serial_port_.IsOpen()) { enc_serial_port_ << "z;"; } } -- 2.49.0