Merge pull request #79 from ibis-ssl/feature/obstacle_avoidance

Feature/obstacle avoidance

Author: HansRobo

crane_bringup/launch/simple_ai.launch.xml

@@ -22,7 +22,7 @@
     <node pkg="crane_local_planner" exec="crane_local_planner_node" output="screen">
         <param name="planner" value="simple"/>
         <param name="non_rvo_p_gain" value="2.0"/>
-        <param name="non_rvo_i_gain" value="0.1"/>
+        <param name="non_rvo_i_gain" value="0.0"/>
         <param name="non_rvo_d_gain" value="0.0"/>
     </node>
 

crane_local_planner/include/crane_local_planner/local_planner.hpp

@@ -17,6 +17,7 @@
 
 #include "gridmap_planner.hpp"
 #include "rvo_planner.hpp"
+#include "simple_avoid_planner.hpp"
 #include "simple_planner.hpp"
 #include "visibility_control.h"
 
@@ -45,6 +46,12 @@ class LocalPlannerComponent : public rclcpp::Node
         [this](const crane_msgs::msg::RobotCommands & msg) -> crane_msgs::msg::RobotCommands {
         return simple_planner->calculateControlTarget(msg, world_model);
       };
+    } else if (planner_str == "simple_avoid") {
+      simple_avoid_planner = std::make_shared<SimpleAvoidPlanner>(*this);
+      calculate_control_target =
+        [this](const crane_msgs::msg::RobotCommands & msg) -> crane_msgs::msg::RobotCommands {
+        return simple_avoid_planner->calculateControlTarget(msg, world_model);
+      };
     } else if (planner_str == "rvo") {
       rvo_planner = std::make_shared<RVOPlanner>(*this);
       calculate_control_target =
@@ -83,6 +90,7 @@ class LocalPlannerComponent : public rclcpp::Node
     calculate_control_target;
 
   std::shared_ptr<SimplePlanner> simple_planner = nullptr;
+  std::shared_ptr<SimpleAvoidPlanner> simple_avoid_planner = nullptr;
   std::shared_ptr<RVOPlanner> rvo_planner = nullptr;
   std::shared_ptr<GridMapPlanner> gridmap_planner = nullptr;
 };