Added dwm rotate

AUTHORS

@@ -1,5 +0,0 @@
-# Authors of Magrob
-#
-# The following individuals and organizations hold copyright for this project:
-
-Pimpest

LICENSE

@@ -1,201 +0,0 @@
-                                 Apache License
-                           Version 2.0, January 2004
-                        http://www.apache.org/licenses/
-
-   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
-
-   1. Definitions.
-
-      "License" shall mean the terms and conditions for use, reproduction,
-      and distribution as defined by Sections 1 through 9 of this document.
-
-      "Licensor" shall mean the copyright owner or entity authorized by
-      the copyright owner that is granting the License.
-
-      "Legal Entity" shall mean the union of the acting entity and all
-      other entities that control, are controlled by, or are under common
-      control with that entity. For the purposes of this definition,
-      "control" means (i) the power, direct or indirect, to cause the
-      direction or management of such entity, whether by contract or
-      otherwise, or (ii) ownership of fifty percent (50%) or more of the
-      outstanding shares, or (iii) beneficial ownership of such entity.
-
-      "You" (or "Your") shall mean an individual or Legal Entity
-      exercising permissions granted by this License.
-
-      "Source" form shall mean the preferred form for making modifications,
-      including but not limited to software source code, documentation
-      source, and configuration files.
-
-      "Object" form shall mean any form resulting from mechanical
-      transformation or translation of a Source form, including but
-      not limited to compiled object code, generated documentation,
-      and conversions to other media types.
-
-      "Work" shall mean the work of authorship, whether in Source or
-      Object form, made available under the License, as indicated by a
-      copyright notice that is included in or attached to the work
-      (an example is provided in the Appendix below).
-
-      "Derivative Works" shall mean any work, whether in Source or Object
-      form, that is based on (or derived from) the Work and for which the
-      editorial revisions, annotations, elaborations, or other modifications
-      represent, as a whole, an original work of authorship. For the purposes
-      of this License, Derivative Works shall not include works that remain
-      separable from, or merely link (or bind by name) to the interfaces of,
-      the Work and Derivative Works thereof.
-
-      "Contribution" shall mean any work of authorship, including
-      the original version of the Work and any modifications or additions
-      to that Work or Derivative Works thereof, that is intentionally
-      submitted to Licensor for inclusion in the Work by the copyright owner
-      or by an individual or Legal Entity authorized to submit on behalf of
-      the copyright owner. For the purposes of this definition, "submitted"
-      means any form of electronic, verbal, or written communication sent
-      to the Licensor or its representatives, including but not limited to
-      communication on electronic mailing lists, source code control systems,
-      and issue tracking systems that are managed by, or on behalf of, the
-      Licensor for the purpose of discussing and improving the Work, but
-      excluding communication that is conspicuously marked or otherwise
-      designated in writing by the copyright owner as "Not a Contribution."
-
-      "Contributor" shall mean Licensor and any individual or Legal Entity
-      on behalf of whom a Contribution has been received by Licensor and
-      subsequently incorporated within the Work.
-
-   2. Grant of Copyright License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      copyright license to reproduce, prepare Derivative Works of,
-      publicly display, publicly perform, sublicense, and distribute the
-      Work and such Derivative Works in Source or Object form.
-
-   3. Grant of Patent License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      (except as stated in this section) patent license to make, have made,
-      use, offer to sell, sell, import, and otherwise transfer the Work,
-      where such license applies only to those patent claims licensable
-      by such Contributor that are necessarily infringed by their
-      Contribution(s) alone or by combination of their Contribution(s)
-      with the Work to which such Contribution(s) was submitted. If You
-      institute patent litigation against any entity (including a
-      cross-claim or counterclaim in a lawsuit) alleging that the Work
-      or a Contribution incorporated within the Work constitutes direct
-      or contributory patent infringement, then any patent licenses
-      granted to You under this License for that Work shall terminate
-      as of the date such litigation is filed.
-
-   4. Redistribution. You may reproduce and distribute copies of the
-      Work or Derivative Works thereof in any medium, with or without
-      modifications, and in Source or Object form, provided that You
-      meet the following conditions:
-
-      (a) You must give any other recipients of the Work or
-          Derivative Works a copy of this License; and
-
-      (b) You must cause any modified files to carry prominent notices
-          stating that You changed the files; and
-
-      (c) You must retain, in the Source form of any Derivative Works
-          that You distribute, all copyright, patent, trademark, and
-          attribution notices from the Source form of the Work,
-          excluding those notices that do not pertain to any part of
-          the Derivative Works; and
-
-      (d) If the Work includes a "NOTICE" text file as part of its
-          distribution, then any Derivative Works that You distribute must
-          include a readable copy of the attribution notices contained
-          within such NOTICE file, excluding those notices that do not
-          pertain to any part of the Derivative Works, in at least one
-          of the following places: within a NOTICE text file distributed
-          as part of the Derivative Works; within the Source form or
-          documentation, if provided along with the Derivative Works; or,
-          within a display generated by the Derivative Works, if and
-          wherever such third-party notices normally appear. The contents
-          of the NOTICE file are for informational purposes only and
-          do not modify the License. You may add Your own attribution
-          notices within Derivative Works that You distribute, alongside
-          or as an addendum to the NOTICE text from the Work, provided
-          that such additional attribution notices cannot be construed
-          as modifying the License.
-
-      You may add Your own copyright statement to Your modifications and
-      may provide additional or different license terms and conditions
-      for use, reproduction, or distribution of Your modifications, or
-      for any such Derivative Works as a whole, provided Your use,
-      reproduction, and distribution of the Work otherwise complies with
-      the conditions stated in this License.
-
-   5. Submission of Contributions. Unless You explicitly state otherwise,
-      any Contribution intentionally submitted for inclusion in the Work
-      by You to the Licensor shall be under the terms and conditions of
-      this License, without any additional terms or conditions.
-      Notwithstanding the above, nothing herein shall supersede or modify
-      the terms of any separate license agreement you may have executed
-      with Licensor regarding such Contributions.
-
-   6. Trademarks. This License does not grant permission to use the trade
-      names, trademarks, service marks, or product names of the Licensor,
-      except as required for reasonable and customary use in describing the
-      origin of the Work and reproducing the content of the NOTICE file.
-
-   7. Disclaimer of Warranty. Unless required by applicable law or
-      agreed to in writing, Licensor provides the Work (and each
-      Contributor provides its Contributions) on an "AS IS" BASIS,
-      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-      implied, including, without limitation, any warranties or conditions
-      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-      PARTICULAR PURPOSE. You are solely responsible for determining the
-      appropriateness of using or redistributing the Work and assume any
-      risks associated with Your exercise of permissions under this License.
-
-   8. Limitation of Liability. In no event and under no legal theory,
-      whether in tort (including negligence), contract, or otherwise,
-      unless required by applicable law (such as deliberate and grossly
-      negligent acts) or agreed to in writing, shall any Contributor be
-      liable to You for damages, including any direct, indirect, special,
-      incidental, or consequential damages of any character arising as a
-      result of this License or out of the use or inability to use the
-      Work (including but not limited to damages for loss of goodwill,
-      work stoppage, computer failure or malfunction, or any and all
-      other commercial damages or losses), even if such Contributor
-      has been advised of the possibility of such damages.
-
-   9. Accepting Warranty or Additional Liability. While redistributing
-      the Work or Derivative Works thereof, You may choose to offer,
-      and charge a fee for, acceptance of support, warranty, indemnity,
-      or other liability obligations and/or rights consistent with this
-      License. However, in accepting such obligations, You may act only
-      on Your own behalf and on Your sole responsibility, not on behalf
-      of any other Contributor, and only if You agree to indemnify,
-      defend, and hold each Contributor harmless for any liability
-      incurred by, or claims asserted against, such Contributor by reason
-      of your accepting any such warranty or additional liability.
-
-   END OF TERMS AND CONDITIONS
-
-   APPENDIX: How to apply the Apache License to your work.
-
-      To apply the Apache License to your work, attach the following
-      boilerplate notice, with the fields enclosed by brackets "[]"
-      replaced with your own identifying information. (Don't include
-      the brackets!)  The text should be enclosed in the appropriate
-      comment syntax for the file format. We also recommend that a
-      file or class name and description of purpose be included on the
-      same "printed page" as the copyright notice for easier
-      identification within third-party archives.
-
-   Copyright [yyyy] [name of copyright owner]
-
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.

mg_control/include/mg_control/mg_control.hpp

@@ -1,17 +1,3 @@
-/** Copyright 2025 The Magrob Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 #ifndef MG_WHEEL_INTERFACE_HPP_
 #define MG_WHEEL_INTERFACE_HPP_
 #include <vector>

mg_control/src/mg_control.cpp

@@ -1,17 +1,3 @@
-/** Copyright 2025 The Magrob Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 #include "mg_control/mg_control.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 

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/package.xml

@@ -5,7 +5,7 @@
   <version>0.0.0</version>
   <description>This contains various msg/action used within the project</description>
   <maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
-  <license>Apache-2.0</license>
+  <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

mg_msgs/srv/SetWidth.srv

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

mg_navigation/include/handlers/dwm.hpp

@@ -1,17 +1,3 @@
-/** Copyright 2025 The Magrob Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 #pragma once
 
 #include "handlers/dwm_core.hpp"

mg_navigation/include/handlers/dwm_core.hpp

@@ -1,17 +1,3 @@
-/** Copyright 2025 The Magrob Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 #pragma once
 
 #include "glm/glm.hpp"

mg_navigation/include/handlers/dwm_forward.hpp

@@ -1,17 +1,3 @@
-/** Copyright 2025 The Magrob Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 #pragma once
 #include "handlers/dwm_core.hpp"
 

mg_navigation/include/handlers/dwm_lookat.hpp

@@ -1,17 +1,3 @@
-/** Copyright 2025 The Magrob Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 #pragma once
 #include "handlers/dwm_core.hpp"
 

mg_navigation/include/handlers/dwm_point.hpp

@@ -1,17 +1,3 @@
-/** Copyright 2025 The Magrob Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 #pragma once
 #include "handlers/dwm_core.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

@@ -1,17 +1,3 @@
-/** Copyright 2025 The Magrob Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 #pragma once
 
 #include <thread>
@@ -22,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"
@@ -40,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

@@ -1,17 +1,3 @@
-/** Copyright 2025 The Magrob Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 #include <iostream>
 #include <functional>
 #include <thread>
@@ -74,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",
@@ -81,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_goal<Rotate>, this, _1, _2, "MoveStraight"),
-          std::bind(&MgNavigationServer::handle_cancel<MoveStraight>, this, _1, "MoveStraight"),
+          std::bind(&MgNavigationServer::handle_cancel<Rotate>, this, _1, "MoveStraight"),
-          std::bind(&MgNavigationServer::handle_accepted<MoveStraight>, this, _1, "MoveStraight"));
+          std::bind(&MgNavigationServer::handle_accepted<Rotate>, this, _1, "MoveStraight"));
     }
 
 };

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

@@ -5,16 +5,19 @@
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
-  <license>Apache-2.0</license>
+  <license>TODO: License declaration</license>
 
   <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,23 +1,15 @@
-/** Copyright 2025 The Magrob Authors
+#include <array>
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
 #include <chrono>
+#include <cstddef>
 #include <functional>
 #include <memory>
+#include <rclcpp/service.hpp>
 #include <string>
 #include <libserial/SerialStream.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"
 
@@ -32,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") {
@@ -43,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<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<double, 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;