Add aditional services to mg_odometry and dwm handlers for easier callibration' (#2)

mg_msgs/CMakeLists.txt

@@ -14,6 +14,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "action/MoveStraight.action"
   "action/MovePoint.action"
   "action/LookAt.action"
+  "action/Rotate.action"
+  "srv/SetWidth.srv"
 )
 
 ament_package()

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

mg_msgs/srv/SetWidth.srv

@@ -0,0 +1,2 @@
+float64 width
+---

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"

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 <glm/gtc/constants.hpp>
+
+namespace mg {
+
+    template <>
+    inline bool DWM<MgNavigationServer::Rotate>::target_check() {
+
+        const double a = glm::abs(theta - target_ornt);
+        const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
+
+        return b > goal->tolerance;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::Rotate>::target_init() {
+        target_ornt = goal->angle;
+    }
+
+    template <>
+    inline bool DWM<MgNavigationServer::Rotate>::condition_check() {
+        return false;
+    }
+
+    template <>
+    inline double DWM<MgNavigationServer::Rotate>::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<double>()) ? glm::two_pi<double>() - dist_old : dist_old;
+        dist_new = (dist_new > glm::pi<double>()) ? glm::two_pi<double>() - dist_new : dist_new;
+
+        const double score = goal->ornt_mult * (dist_old - dist_new);
+
+        return score;
+    }
+
+    template <>
+    inline void DWM<MgNavigationServer::Rotate>::populate_candidates(std::vector<int>& vl,
+                                                                     std::vector<int>& 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);
+            }
+        }
+    }
+
+}

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<Geometry::TwistStamped>::SharedPtr pub_twist;
 
-        rclcpp_action::Server<LookAt>::SharedPtr       sv_look_at;
         rclcpp_action::Server<MovePoint>::SharedPtr    sv_move_point;
         rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;
+        rclcpp_action::Server<LookAt>::SharedPtr       sv_look_at;
+        rclcpp_action::Server<Rotate>::SharedPtr       sv_rotate;
 
         rclcpp::Subscription<Geometry::TransformStamped>::SharedPtr tf2_subscription_;
         tf2_ros::Buffer::SharedPtr                                  tf2_buffer;

mg_navigation/src/mg_navigation.cpp

@@ -60,6 +60,13 @@ namespace mg {
           std::bind(&MgNavigationServer::handle_cancel<MovePoint>, this, _1, "MovePoint"),
           std::bind(&MgNavigationServer::handle_accepted<MovePoint>, this, _1, "MovePoint"));
 
+        sv_move_straight = rclcpp_action::create_server<MoveStraight>(
+          this,
+          "MoveStraight",
+          std::bind(&MgNavigationServer::handle_goal<MoveStraight>, this, _1, _2, "MoveStraight"),
+          std::bind(&MgNavigationServer::handle_cancel<MoveStraight>, this, _1, "MoveStraight"),
+          std::bind(&MgNavigationServer::handle_accepted<MoveStraight>, this, _1, "MoveStraight"));
+
         sv_look_at = rclcpp_action::create_server<LookAt>(
           this,
           "LookAt",
@@ -67,12 +74,12 @@ namespace mg {
           std::bind(&MgNavigationServer::handle_cancel<LookAt>, this, _1, "LookAt"),
           std::bind(&MgNavigationServer::handle_accepted<LookAt>, this, _1, "LookAt"));
 
-        sv_move_straight = rclcpp_action::create_server<MoveStraight>(
+        sv_rotate = rclcpp_action::create_server<Rotate>(
           this,
-          "MoveStraight",
-          std::bind(&MgNavigationServer::handle_goal<MoveStraight>, this, _1, _2, "MoveStraight"),
-          std::bind(&MgNavigationServer::handle_cancel<MoveStraight>, this, _1, "MoveStraight"),
-          std::bind(&MgNavigationServer::handle_accepted<MoveStraight>, this, _1, "MoveStraight"));
+          "Rotate",
+          std::bind(&MgNavigationServer::handle_goal<Rotate>, this, _1, _2, "Rotate"),
+          std::bind(&MgNavigationServer::handle_cancel<Rotate>, this, _1, "Rotate"),
+          std::bind(&MgNavigationServer::handle_accepted<Rotate>, this, _1, "Rotate"));
     }
 
 };

mg_odometry/CMakeLists.txt

@@ -11,6 +11,8 @@ 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)
 
@@ -23,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

mg_odometry/package.xml

@@ -10,11 +10,14 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
+  <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>libserial-dev</depend>
 
+  <depend>mg_msgs</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

mg_odometry/src/mg_odom_publisher.cpp

@@ -1,8 +1,15 @@
+#include <array>
 #include <chrono>
+#include <cstddef>
 #include <functional>
 #include <memory>
+#include <rclcpp/service.hpp>
 #include <string>
 #include <libserial/SerialStream.h>
+#include <sys/types.h>
+
+#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 +25,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 +39,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<SetWidthSrv>(
+          "/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
+
+        zero_service_
+          = create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
     }
 
   private:
     std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+    rclcpp::Service<SetWidthSrv>::SharedPtr        set_width_service_;
+    rclcpp::Service<ZeroSrv>::SharedPtr            zero_service_;
     rclcpp::TimerBase::SharedPtr                   timer_;
     LibSerial::SerialStream                        enc_serial_port_;
 
+    void set_width(const std::shared_ptr<SetWidthSrv::Request> request) {
+        RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width);
+
+        if (enc_serial_port_.IsOpen()) {
+            union {
+                std::array<u_char, sizeof(double)> 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<ZeroSrv::Request> /*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;