init stuck_behavior

Author: VladimirVincan

mep3_navigation/CMakeLists.txt

@@ -140,6 +140,33 @@ install(
 target_link_libraries(move_behavior ruckig::ruckig)
 pluginlib_export_plugin_description_file(nav2_core move_behavior.xml)
 
+# stuck_behavior
+add_library(
+  stuck_behavior
+  SHARED
+  src/stuck_behavior/stuck_behavior.cpp
+)
+ament_target_dependencies(
+  stuck_behavior
+  rclcpp
+  nav2_costmap_2d
+  pluginlib
+  rclcpp_action
+  mep3_msgs
+  can_msgs
+)
+target_include_directories(
+  stuck_behavior
+  PRIVATE
+  include
+)
+install(
+  TARGETS stuck_behavior
+  DESTINATION lib
+)
+target_link_libraries(stuck_behavior ruckig::ruckig)
+pluginlib_export_plugin_description_file(nav2_core stuck_behavior.xml)
+
 # Assets
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY params DESTINATION share/${PROJECT_NAME})

mep3_navigation/include/mep3_navigation/stuck_behavior/stuck_behavior.hpp

@@ -0,0 +1,33 @@
+#ifndef MEP3_NAVIGATION__STUCK_BEHAVIOR_HPP_
+#define MEP3_NAVIGATION__STUCK_BEHAVIOR_HPP_
+
+#include <chrono>
+#include <memory>
+#include <utility>
+
+#include "nav2_behaviors/timed_behavior.hpp"
+#include "mep3_msgs/action/move.hpp"
+#include "mep3_msgs/msg/motion_properties.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "ruckig/ruckig.hpp"
+#include "mep3_navigation/stuck_detector.hpp"
+
+namespace mep3_navigation
+{
+  using ActionT = mep3_msgs::action::Move;
+
+  class StuckBehavior : public nav2_behaviors::TimedBehavior<ActionT>
+  {
+  public:
+    StuckBehavior();
+    ~StuckBehavior();
+
+    virtual nav2_behaviors::Status onRun(const std::shared_ptr<const typename ActionT::Goal> command) final override;
+
+    nav2_behaviors::Status onCycleUpdate();
+
+  protected:
+  };
+}
+
+#endif // MEP3_NAVIGATION__STUCK_BEHAVIOR_HPP_

mep3_navigation/package.xml

@@ -9,7 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_python</buildtool_depend>
-  
+
   <build_depend>navigation2</build_depend>
   <build_depend>nav2_common</build_depend>
 

mep3_navigation/src/stuck_behavior/stuck_behavior.cpp

@@ -0,0 +1,21 @@
+#include "mep3_navigation/stuck_behavior/stuck_behavior.hpp"
+
+
+mep3_navigation::StuckBehavior::StuckBehavior() :
+  nav2_behaviors::TimedBehavior<ActionT>()
+{
+}
+
+mep3_navigation::StuckBehavior::~StuckBehavior() = default;
+
+nav2_behaviors::Status mep3_navigation::StuckBehavior::onRun(const std::shared_ptr<const typename ActionT::Goal> command)
+{
+  RCLCPP_INFO(this->logger_, "onRun successful!");
+  return nav2_behaviors::Status::SUCCEEDED;
+}
+
+nav2_behaviors::Status mep3_navigation::StuckBehavior::onCycleUpdate()
+{
+  RCLCPP_INFO(this->logger_, "onCycleUpdate successful!");
+  return nav2_behaviors::Status::RUNNING;
+}

mep3_navigation/stuck_behavior.xml

@@ -0,0 +1,5 @@
+<library path="stuck_behavior">
+  <class type="mep3_navigation::StuckBehavior" base_class_type="nav2_core::Behavior">
+    <description>Provides primitive but flexible robot motion stuck behavior.</description>
+  </class>
+</library>