Merge pull request #181 from cogip/180-planner-allow-forcing-backward-motions

180 planner allow forcing backward motions

Author: ecourtois

cogip/cpp/libraries/models/PoseOrder.cpp

@@ -26,7 +26,7 @@ PoseOrder::PoseOrder(
     double angle,
     std::uint8_t max_speed_linear,
     std::uint8_t max_speed_angular,
-    bool allow_reverse,
+    MotionDirection motion_direction,
     bool bypass_anti_blocking,
     bool bypass_final_orientation,
     std::uint32_t timeout_ms,
@@ -42,7 +42,7 @@ PoseOrder::PoseOrder(
     data_->angle = angle;
     data_->max_speed_linear = max_speed_linear;
     data_->max_speed_angular = max_speed_angular;
-    data_->allow_reverse = allow_reverse;
+    data_->motion_direction = motion_direction;
     data_->bypass_anti_blocking = bypass_anti_blocking;
     data_->bypass_final_orientation = bypass_final_orientation;
     data_->timeout_ms = timeout_ms;

cogip/cpp/libraries/models/PoseOrderList.cpp

@@ -14,7 +14,7 @@ void PoseOrderList::append(
     double angle,
     std::uint8_t max_speed_linear,
     std::uint8_t max_speed_angular,
-    bool allow_reverse,
+    MotionDirection motion_direction,
     bool bypass_anti_blocking,
     bool bypass_final_orientation,
     std::uint32_t timeout_ms,
@@ -29,7 +29,7 @@ void PoseOrderList::append(
     list_->elems[list_->count].angle = angle;
     list_->elems[list_->count].max_speed_linear = max_speed_linear;
     list_->elems[list_->count].max_speed_angular = max_speed_angular;
-    list_->elems[list_->count].allow_reverse = allow_reverse;
+    list_->elems[list_->count].motion_direction = motion_direction;
     list_->elems[list_->count].bypass_anti_blocking = bypass_anti_blocking;
     list_->elems[list_->count].bypass_final_orientation = bypass_final_orientation;
     list_->elems[list_->count].timeout_ms = timeout_ms;
@@ -44,7 +44,7 @@ void PoseOrderList::set(
     double angle,
     std::uint8_t max_speed_linear,
     std::uint8_t max_speed_angular,
-    bool allow_reverse,
+    MotionDirection motion_direction,
     bool bypass_anti_blocking,
     bool bypass_final_orientation,
     std::uint32_t timeout_ms,
@@ -58,7 +58,7 @@ void PoseOrderList::set(
     list_->elems[index].angle = angle;
     list_->elems[index].max_speed_linear = max_speed_linear;
     list_->elems[index].max_speed_angular = max_speed_angular;
-    list_->elems[index].allow_reverse = allow_reverse;
+    list_->elems[index].motion_direction = motion_direction;
     list_->elems[index].bypass_anti_blocking = bypass_anti_blocking;
     list_->elems[index].bypass_final_orientation = bypass_final_orientation;
     list_->elems[index].timeout_ms = timeout_ms;

cogip/cpp/libraries/models/binding.cpp

@@ -29,6 +29,13 @@ NB_MODULE(models, m) {
 
     m.doc() = "models module for Python bindings";
 
+    // Bind MotionDirection enum
+    nb::enum_<MotionDirection>(m, "MotionDirection", "Motion direction mode for path navigation")
+        .value("bidirectional", MotionDirection::bidirectional, "Robot can move forward or backward (choose optimal)")
+        .value("forward_only", MotionDirection::forward_only, "Force forward motion only")
+        .value("backward_only", MotionDirection::backward_only, "Force backward motion only")
+        .export_values();
+
     // Bind coords_t structure
     nb::class_<coords_t>(m, "CoordsT")
         .def_rw("x", &coords_t::x, "X-coordinate")
@@ -204,7 +211,7 @@ NB_MODULE(models, m) {
         .def_rw("angle", &pose_order_t::angle, "Orientation angle of the pose in degrees")
         .def_rw("max_speed_linear", &pose_order_t::max_speed_linear, "Maximum linear speed for the pose (in percent of the robot max speed)")
         .def_rw("max_speed_angular", &pose_order_t::max_speed_angular, "Maximum angular speed for the pose (in percent of the robot max speed)")
-        .def_rw("allow_reverse", &pose_order_t::allow_reverse, "True if the pose allows reverse movement, false otherwise")
+        .def_rw("motion_direction", &pose_order_t::motion_direction, "Motion direction mode (bidirectional, forward_only, or backward_only)")
         .def_rw("bypass_anti_blocking", &pose_order_t::bypass_anti_blocking, "True if the pose bypasses anti-blocking, false otherwise")
         .def_rw("bypass_final_orientation", &pose_order_t::bypass_final_orientation, "True if the pose bypasses final orientation, false otherwise")
         .def_rw("timeout_ms", &pose_order_t::timeout_ms, "Timeout in milliseconds for the pose to be reached")
@@ -219,14 +226,14 @@ NB_MODULE(models, m) {
     // Bind PoseOrder class
     nb::class_<PoseOrder>(m, "PoseOrder")
         .def(
-            nb::init<double, double, double, std::uint8_t, std::uint8_t, bool, bool, bool, std::uint32_t, bool, pose_order_t*>(),
+            nb::init<double, double, double, std::uint8_t, std::uint8_t, MotionDirection, bool, bool, std::uint32_t, bool, pose_order_t*>(),
             "Constructor with initial values",
             "x"_a = 0.0,
             "y"_a = 0.0,
             "angle"_a = 0.0,
             "max_speed_linear"_a = 100,
             "max_speed_angular"_a = 100,
-            "allow_reverse"_a = false,
+            "motion_direction"_a = MotionDirection::bidirectional,
             "bypass_anti_blocking"_a = false,
             "bypass_final_orientation"_a = false,
             "timeout_ms"_a = 0,
@@ -240,7 +247,7 @@ NB_MODULE(models, m) {
         .def_prop_rw("angle", &PoseOrder::angle, &PoseOrder::set_angle, "Get or set the orientation angle")
         .def_prop_rw("max_speed_linear", &PoseOrder::max_speed_linear, &PoseOrder::set_max_speed_linear, "Get or set the maximum linear speed (in percent of the robot max speed)")
         .def_prop_rw("max_speed_angular", &PoseOrder::max_speed_angular, &PoseOrder::set_max_speed_angular, "Get or set the maximum angular speed (in percent of the robot max speed)")
-        .def_prop_rw("allow_reverse", &PoseOrder::allow_reverse, &PoseOrder::set_allow_reverse, "Get or set if reverse movement is allowed")
+        .def_prop_rw("motion_direction", &PoseOrder::motion_direction, &PoseOrder::set_motion_direction, "Get or set the motion direction mode")
         .def_prop_rw("bypass_anti_blocking", &PoseOrder::bypass_anti_blocking, &PoseOrder::set_bypass_anti_blocking, "Get or set if anti-blocking is bypassed")
         .def_prop_rw("bypass_final_orientation", &PoseOrder::bypass_final_orientation, &PoseOrder::set_bypass_final_orientation, "Get or set if final orientation is bypassed")
         .def_prop_rw("timeout_ms", &PoseOrder::timeout_ms, &PoseOrder::set_timeout_ms, "Get or set the timeout in milliseconds for the pose to be reached")
@@ -276,27 +283,27 @@ NB_MODULE(models, m) {
         .def("max_size", &PoseOrderList::max_size, "Get the maximum number of pose orders")
         .def("get", &PoseOrderList::get, "Get PoseOrder at index", "index"_a)
         .def("__getitem__", &PoseOrderList::operator[], "Get PoseOrder at index", "index"_a)
-        .def("append", nb::overload_cast<double, double, double, std::uint8_t, std::uint8_t, bool, bool, bool, std::uint32_t, bool>(&PoseOrderList::append), "Append PoseOrder with initial values",
+        .def("append", nb::overload_cast<double, double, double, std::uint8_t, std::uint8_t, MotionDirection, bool, bool, std::uint32_t, bool>(&PoseOrderList::append), "Append PoseOrder with initial values",
             "x"_a = 0.0,
             "y"_a = 0.0,
             "angle"_a = 0.0,
             "max_speed_linear"_a = 100,
             "max_speed_angular"_a = 100,
-            "allow_reverse"_a = false,
+            "motion_direction"_a = MotionDirection::bidirectional,
             "bypass_anti_blocking"_a = false,
             "bypass_final_orientation"_a = false,
             "timeout_ms"_a = 0,
             "is_intermediate"_a = false
         )
         .def("append", nb::overload_cast<const PoseOrder&>(&PoseOrderList::append), "Append PoseOrder object", "pose_order"_a)
-        .def("set", nb::overload_cast<std::size_t, double, double, double, std::uint8_t, std::uint8_t, bool, bool, bool, std::uint32_t, bool>(&PoseOrderList::set), "Set PoseOrder at index",
+        .def("set", nb::overload_cast<std::size_t, double, double, double, std::uint8_t, std::uint8_t, MotionDirection, bool, bool, std::uint32_t, bool>(&PoseOrderList::set), "Set PoseOrder at index",
             "index"_a,
             "x"_a = 0.0,
             "y"_a = 0.0,
             "angle"_a = 0.0,
             "max_speed_linear"_a = 100,
             "max_speed_angular"_a = 100,
-            "allow_reverse"_a = false,
+            "motion_direction"_a = MotionDirection::bidirectional,
             "bypass_anti_blocking"_a = false,
             "bypass_final_orientation"_a = false,
             "timeout_ms"_a = 0,

cogip/cpp/libraries/models/include/models/PoseOrder.hpp

@@ -37,7 +37,7 @@ class PoseOrder {
         double angle=0.0,                     ///< [in] orientation
         std::uint8_t max_speed_linear=66,     ///< [in] Maximum linear speed for the pose (in percent of the robot max speed).
         std::uint8_t max_speed_angular=66,    ///< [in] Maximum linear speed for the pose (in percent of the robot max speed).
-        bool allow_reverse=true,              ///< [in] True if the pose allows reverse movement, false otherwise.
+        MotionDirection motion_direction=MotionDirection::bidirectional,  ///< [in] Motion direction mode.
         bool bypass_anti_blocking=false,      ///< [in] True if the pose bypasses anti-blocking, false otherwise.
         bool bypass_final_orientation=false,  ///< [in] True if the pose bypasses final orientation, false otherwise.
         std::uint32_t timeout_ms=0,           ///< [in] Timeout in milliseconds for the pose to be reached.
@@ -91,13 +91,13 @@ class PoseOrder {
         std::uint8_t max_speed_angular  ///< [in] new maximum angular speed for the pose (in percent of the robot max speed)
     ) { data_->max_speed_angular = max_speed_angular; }
 
-    /// Return true if the pose allows reverse movement, false otherwise.
-    bool allow_reverse(void) const { return data_->allow_reverse; }
+    /// Return motion direction mode.
+    MotionDirection motion_direction(void) const { return data_->motion_direction; }
 
-    /// Set whether the pose allows reverse movement.
-    void set_allow_reverse(
-        bool allow_reverse  ///< [in] true if the pose allows reverse movement, false otherwise
-    ) { data_->allow_reverse = allow_reverse; }
+    /// Set motion direction mode.
+    void set_motion_direction(
+        MotionDirection motion_dir  ///< [in] motion direction mode (bidirectional, forward_only, or backward_only)
+    ) { data_->motion_direction = motion_dir; }
 
     /// Return true if the pose bypasses anti-blocking, false otherwise.
     bool bypass_anti_blocking(void) const { return data_->bypass_anti_blocking; }
@@ -148,9 +148,9 @@ inline std::ostream& operator<<(std::ostream& os, PoseOrder data) {
        << "x=" << data.x() << ", "
        << "y=" << data.y() << ", "
        << "angle=" << data.angle() << ", "
-       << "max_speed_linear=" << data.max_speed_linear() << ", "
-       << "max_speed_angular=" << data.max_speed_angular() << ", "
-       << "allow_reverse=" << data.allow_reverse() << ", "
+       << "max_speed_linear=" << static_cast<int>(data.max_speed_linear()) << ", "
+       << "max_speed_angular=" << static_cast<int>(data.max_speed_angular()) << ", "
+       << "motion_direction=" << static_cast<int>(data.motion_direction()) << ", "
        << "bypass_anti_blocking=" << data.bypass_anti_blocking() << ", "
        << "bypass_final_orientation=" << data.bypass_final_orientation() << ", "
        << "timeout_ms=" << data.timeout_ms() << ", "

cogip/cpp/libraries/models/include/models/PoseOrderList.hpp

@@ -29,7 +29,7 @@ class PoseOrderList: public List<pose_order_t, PoseOrder, pose_order_list_t, POS
         double angle = 0.0,
         std::uint8_t max_speed_linear = 66,
         std::uint8_t max_speed_angular = 66,
-        bool allow_reverse = true,
+        MotionDirection motion_direction = MotionDirection::bidirectional,
         bool bypass_anti_blocking = false,
         bool bypass_final_orientation = false,
         std::uint32_t timeout_ms = 0,
@@ -42,7 +42,7 @@ class PoseOrderList: public List<pose_order_t, PoseOrder, pose_order_list_t, POS
             elem->angle,
             elem->max_speed_linear,
             elem->max_speed_angular,
-            elem->allow_reverse,
+            elem->motion_direction,
             elem->bypass_anti_blocking,
             elem->bypass_final_orientation,
             elem->timeout_ms,
@@ -56,7 +56,7 @@ class PoseOrderList: public List<pose_order_t, PoseOrder, pose_order_list_t, POS
             elem.angle(),
             elem.max_speed_linear(),
             elem.max_speed_angular(),
-            elem.allow_reverse(),
+            elem.motion_direction(),
             elem.bypass_anti_blocking(),
             elem.bypass_final_orientation(),
             elem.timeout_ms(),
@@ -70,7 +70,7 @@ class PoseOrderList: public List<pose_order_t, PoseOrder, pose_order_list_t, POS
         double angle = 0.0,
         std::uint8_t max_speed_linear = 66,
         std::uint8_t max_speed_angular = 66,
-        bool allow_reverse = true,
+        MotionDirection motion_direction = MotionDirection::bidirectional,
         bool bypass_anti_blocking = false,
         bool bypass_final_orientation = false,
         std::uint32_t timeout_ms = 0,
@@ -84,7 +84,7 @@ class PoseOrderList: public List<pose_order_t, PoseOrder, pose_order_list_t, POS
             elem->angle,
             elem->max_speed_linear,
             elem->max_speed_angular,
-            elem->allow_reverse,
+            elem->motion_direction,
             elem->bypass_anti_blocking,
             elem->bypass_final_orientation,
             elem->timeout_ms,
@@ -99,7 +99,7 @@ class PoseOrderList: public List<pose_order_t, PoseOrder, pose_order_list_t, POS
             elem.angle(),
             elem.max_speed_linear(),
             elem.max_speed_angular(),
-            elem.allow_reverse(),
+            elem.motion_direction(),
             elem.bypass_anti_blocking(),
             elem.bypass_final_orientation(),
             elem.timeout_ms(),

cogip/cpp/libraries/models/include/models/pose_order.hpp

@@ -17,14 +17,21 @@ namespace cogip {
 
 namespace models {
 
+/// Motion direction mode for path navigation.
+enum class MotionDirection : std::uint8_t {
+    bidirectional = 0,  ///< Robot can move forward or backward (choose optimal)
+    forward_only = 1,   ///< Force forward motion only
+    backward_only = 2   ///< Force backward motion only
+};
+
 /// Represents a pose in 2D space with an orientation.
 typedef struct {
     double x;      ///< X-coordinate of the pose.
     double y;      ///< Y-coordinate of the pose.
     double angle;  ///< Orientation angle of the pose in degrees.
     std::uint8_t max_speed_linear; ///< Maximum linear speed for the pose (in percent of the robot max speed).
     std::uint8_t max_speed_angular; ///< Maximum linear speed for the pose (in percent of the robot max speed).
-    bool allow_reverse; ///< True if the pose allows reverse movement, false otherwise.
+    MotionDirection motion_direction; ///< Motion direction mode (bidirectional, forward_only, or backward_only).
     bool bypass_anti_blocking; ///< True if the pose bypasses anti-blocking, false otherwise.
     bool bypass_final_orientation; ///< True if the pose bypasses final orientation, false otherwise.
     std::uint32_t timeout_ms; ///< Timeout in milliseconds for the pose to be reached.
@@ -41,9 +48,9 @@ inline std::ostream& operator<<(std::ostream& os, const pose_order_t& data) {
        << "x=" << data.x << ", "
        << "y=" << data.y << ", "
        << "angle=" << data.angle << ", "
-       << "max_speed_linear=" << data.max_speed_linear << ", "
-       << "max_speed_angular=" << data.max_speed_angular << ", "
-       << "allow_reverse=" << data.allow_reverse << ", "
+       << "max_speed_linear=" << static_cast<int>(data.max_speed_linear) << ", "
+       << "max_speed_angular=" << static_cast<int>(data.max_speed_angular) << ", "
+       << "motion_direction=" << static_cast<int>(data.motion_direction) << ", "
        << "bypass_anti_blocking=" << data.bypass_anti_blocking << ", "
        << "bypass_final_orientation=" << data.bypass_final_orientation << ", "
        << "timeout_ms=" << data.timeout_ms << ", "

cogip/models/models.py

@@ -8,6 +8,7 @@
 """
 
 import math
+from enum import IntEnum
 
 import numpy as np
 from numpy.typing import ArrayLike
@@ -18,6 +19,14 @@
 from cogip.protobuf import PB_PathPose
 
 
+class MotionDirection(IntEnum):
+    """Motion direction mode for path navigation."""
+
+    BIDIRECTIONAL = 0  # Robot can move forward or backward (choose optimal)
+    FORWARD_ONLY = 1  # Force forward motion only
+    BACKWARD_ONLY = 2  # Force backward motion only
+
+
 class MenuEntry(BaseModel):
     """
     Represents one entry in a firmware's shell menu
@@ -118,7 +127,7 @@ class PathPose(Pose):
         O: 0-orientation
         max_speed_linear: max linear speed in percentage of the robot max linear speed
         max_speed_angular: max angular speed in percentage of the robot max angular speed
-        allow_reverse: reverse mode
+        motion_direction: motion direction mode (bidirectional, forward_only, or backward_only)
         bypass_anti_blocking: send pose_reached if robot is blocked
         timeout_ms: max time is milliseconds to reach the pose, the robot stops if timeout is reached, 0 for no timeout
         bypass_final_orientation: do not set orientation pose order
@@ -127,7 +136,7 @@ class PathPose(Pose):
 
     max_speed_linear: int = 66
     max_speed_angular: int = 66
-    allow_reverse: bool = True
+    motion_direction: MotionDirection = MotionDirection.BIDIRECTIONAL
     bypass_anti_blocking: bool = False
     timeout_ms: int = 0
     bypass_final_orientation: bool = False
@@ -149,7 +158,7 @@ def copy_pb(self, pb_path_pose: PB_PathPose) -> None:
         pb_path_pose.pose.O = int(self.O)  # noqa
         pb_path_pose.max_speed_ratio_linear = self.max_speed_linear
         pb_path_pose.max_speed_ratio_angular = self.max_speed_angular
-        pb_path_pose.allow_reverse = self.allow_reverse
+        pb_path_pose.motion_direction = self.motion_direction.value
         pb_path_pose.bypass_anti_blocking = self.bypass_anti_blocking
         pb_path_pose.timeout_ms = self.timeout_ms
         pb_path_pose.bypass_final_orientation = self.bypass_final_orientation
@@ -169,7 +178,7 @@ def to_shared(self, shared_pose_order: SharedPoseOrder | None) -> None:
         shared_pose_order.angle = int(self.O)  # noqa
         shared_pose_order.max_speed_linear = self.max_speed_linear
         shared_pose_order.max_speed_angular = self.max_speed_angular
-        shared_pose_order.allow_reverse = self.allow_reverse
+        shared_pose_order.motion_direction = self.motion_direction.value
         shared_pose_order.bypass_anti_blocking = self.bypass_anti_blocking
         shared_pose_order.bypass_final_orientation = self.bypass_final_orientation
         shared_pose_order.timeout_ms = self.timeout_ms
@@ -194,7 +203,7 @@ def from_shared(cls, shared_pose: SharedPose | SharedPoseOrder) -> "PathPose":
         if isinstance(shared_pose, SharedPoseOrder):
             path_pose.max_speed_linear = shared_pose.max_speed_linear
             path_pose.max_speed_angular = shared_pose.max_speed_angular
-            path_pose.allow_reverse = shared_pose.allow_reverse
+            path_pose.motion_direction = shared_pose.motion_direction
             path_pose.bypass_anti_blocking = shared_pose.bypass_anti_blocking
             path_pose.timeout_ms = shared_pose.timeout_ms
             path_pose.bypass_final_orientation = shared_pose.bypass_final_orientation