diff --git a/.github/workflows/jazzy.yaml b/.github/workflows/jazzy.yaml index 11301a4d..365cdccf 100644 --- a/.github/workflows/jazzy.yaml +++ b/.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: | { diff --git a/.github/workflows/jazzy_cron.yaml b/.github/workflows/jazzy_cron.yaml index ac2a5efd..c7049a98 100644 --- a/.github/workflows/jazzy_cron.yaml +++ b/.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: | { diff --git a/.github/workflows/kilted.yaml b/.github/workflows/kilted.yaml index 5eff233e..0a4f68ed 100644 --- a/.github/workflows/kilted.yaml +++ b/.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: | { diff --git a/.github/workflows/kilted_cron.yaml b/.github/workflows/kilted_cron.yaml index dc93850f..c4b6b82b 100644 --- a/.github/workflows/kilted_cron.yaml +++ b/.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: | { diff --git a/.github/workflows/rolling_cron.yaml b/.github/workflows/rolling_cron.yaml index 740df03b..ef59286d 100644 --- a/.github/workflows/rolling_cron.yaml +++ b/.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 diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index c8c31066..cee5a66d 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/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 & 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). diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index 939839c5..66ec8eea 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -107,14 +107,27 @@ MPCController::collision_checker(void *data, std::vector & 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(¶ms, u); + if (ControllerMethodBase::collision_checker_active_) { + collision_checker(¶ms, u); + } // Final alignment phase with hysteresis on distance: // - Enter when dist_to_goal <= 0.5 * pos_tol diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index 002b28db..33e837e0 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -18,7 +18,6 @@ nav_msgs sensor_msgs ament_index_cpp - octomap octomap_ros octomap_msgs pcl_conversions