Latest Updates and Corrected CI

.github/workflows/jazzy.yaml

@@ -24,9 +24,8 @@ jobs:
       - name: build and test
         uses: ros-tooling/action-ros-ci@0.4.5
         with:
-          package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller
+          package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller
           target-ros2-distro: jazzy
-          vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos
           skip-tests: false
           colcon-defaults: |
             {

.github/workflows/jazzy_cron.yaml

@@ -18,10 +18,8 @@ jobs:
       - name: build and test
         uses: ros-tooling/action-ros-ci@0.4.5
         with:
-          package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller
+          package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller
           target-ros2-distro: jazzy
-          ref: jazzy
-          vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos
           skip-tests: false
           colcon-defaults: |
             {

.github/workflows/kilted.yaml

@@ -22,9 +22,8 @@ jobs:
       - name: build and test
         uses: ros-tooling/action-ros-ci@0.4.5
         with:
-          package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller
+          package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller
           target-ros2-distro: kilted
-          vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos
           skip-tests: false
           colcon-defaults: |
             {

.github/workflows/kilted_cron.yaml

@@ -18,10 +18,8 @@ jobs:
       - name: build and test
         uses: ros-tooling/action-ros-ci@0.4.5
         with:
-          package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller
+          package-name: easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller
           target-ros2-distro: kilted
-          ref: kilted
-          vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos
           skip-tests: false
           colcon-defaults: |
             {

.github/workflows/rolling_cron.yaml

@@ -18,7 +18,7 @@ jobs:
       - name: build and test
         uses: ros-tooling/action-ros-ci@0.4.5
         with:
-          package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller
+          package-name: easynav_common easynav_controller easynav_core easynav_interfaces easynav_localizer easynav_maps_manager easynav_planner easynav_sensors easynav_support_py easynav_system easynav_tools easynav_bonxai_maps_manager easynav_costmap_common easynav_costmap_localizer easynav_costmap_maps_manager easynav_costmap_planner easynav_gps_localizer easynav_fusion_localizer easynav_mppi_controller easynav_navmap_localizer easynav_navmap_maps_manager easynav_navmap_planner easynav_serest_controller easynav_simple_common easynav_simple_controller easynav_simple_localizer easynav_simple_maps_manager easynav_simple_planner easynav_vff_controller easynav_mpc_controller
           target-ros2-distro: rolling
           ref: rolling
           vcs-repo-file-url: ${GITHUB_WORKSPACE}/.github/thirdparty.repos

controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp

@@ -66,12 +66,16 @@ class MPCController : public ControllerMethodBase
   void collision_checker(void *data, std::vector<double> & u);
 
 protected:
-  int horizon_steps_{5};        ///< Prediction horizon for MPC.
-  double dt_{0.1};              ///< Time step for MPC.
-  double safety_radius_{0.35};  ///< Safety radius to avoid obstacles
-  double max_lin_vel_{1.5};     ///< Maximum linear velocity for MPC.
-  double max_ang_vel_{1.5};     ///< Maximum angular velocity for MPC.
-  bool verbose_{false};         ///< Value to debug on terminal
+  int horizon_steps_{5};                ///< Prediction horizon for MPC.
+  double dt_{0.1};                      ///< Time step for MPC.
+  double safety_radius_{0.35};          ///< Safety radius to avoid obstacles
+  double max_lin_vel_{1.5};             ///< Maximum linear velocity for MPC.
+  double max_ang_vel_{1.5};             ///< Maximum angular velocity for MPC.
+  bool verbose_{false};                 ///< Value to debug on terminal
+  double last_v_{0.0};                  ///< Last value for linear velocity before than collision
+  double last_w_{0.0};                  ///< Last value for angular velocity before than collision
+  bool collision_state_{false};         ///< Collision state flag
+  double collision_factor_{0.618033};   ///< Collision avoidance for recalculate velocities
 
   // Fallback goal tolerances if GoalManager does not publish them
   double fallback_goal_pos_tol_{0.05};   ///< Default positional tolerance (meters).

controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp

@@ -107,14 +107,27 @@ MPCController::collision_checker(void *data, std::vector<double> & u)
       real_points++;
     }
   }
-  x_m /= real_points;
-  y_m /= real_points;
-  dist = std::hypot(x_m, y_m);
-  angle = std::atan2(y_m, x_m) - params->theta0[2];
-  if(real_points != 0 && dist < safety_radius_) {
-    std::cerr << "Detection at: " << dist << " Theta: " << angle << std::endl;
-    u[0] -= dist / real_points * dt_;
-    u[1] -= angle / real_points * dt_;
+  if(real_points != 0) {
+    x_m /= real_points;
+    y_m /= real_points;
+    dist = std::hypot(x_m, y_m);
+    angle = std::atan2(y_m, x_m) - params->theta0[2];
+    if(dist < safety_radius_) {
+      if(!collision_state_) {
+        collision_state_ = true;
+        last_v_ = u[0];
+        last_w_ = u[1];
+      }
+      RCLCPP_WARN(get_node()->get_logger(),
+        "[COLLISION] Collision detected at: [%f] m and Theta: [%f] degrees", dist,
+        (angle * 180.00 / M_PI) );
+      last_v_ = collision_factor_ * last_v_ * safety_radius_ / dist;
+      last_w_ = collision_factor_ * last_w_ * safety_radius_ / dist;
+      u[0] = -last_v_;
+      u[1] = -last_w_;
+    } else {
+      collision_state_ = false;
+    }
   }
 }
 
@@ -273,7 +286,9 @@ MPCController::update_rt(NavState & nav_state)
     std::cerr << "Optimization Error: " << e.what() << std::endl;
   }
 
-  collision_checker(&params, u);
+  if (ControllerMethodBase::collision_checker_active_) {
+    collision_checker(&params, u);
+  }
 
   // Final alignment phase with hysteresis on distance:
   // - Enter when dist_to_goal <= 0.5 * pos_tol

maps_managers/easynav_octomap_maps_manager/package.xml

@@ -18,7 +18,6 @@
   <depend>nav_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>ament_index_cpp</depend>
-  <depend>octomap</depend>
   <depend>octomap_ros</depend>
   <depend>octomap_msgs</depend>
   <depend>pcl_conversions</depend>