From 1cd77b558220fb4009b985246bda97bbb95e8405 Mon Sep 17 00:00:00 2001 From: Pradheep Date: Wed, 3 Dec 2025 06:48:14 +0100 Subject: [PATCH 01/13] adding kilted support --- include/NeoMpcPlanner.h | 15 ++++++++------- src/NeoMpcPlanner.cpp | 16 ++++++++-------- 2 files changed, 16 insertions(+), 15 deletions(-) diff --git a/include/NeoMpcPlanner.h b/include/NeoMpcPlanner.h index edf0622..8f54ab2 100644 --- a/include/NeoMpcPlanner.h +++ b/include/NeoMpcPlanner.h @@ -38,8 +38,8 @@ SOFTWARE. #include "nav2_core/controller.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_costmap_2d/footprint_collision_checker.hpp" -#include "nav2_ros_common/lifecycle_node.hpp" -#include "nav2_ros_common/service_client.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_client.hpp" #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" @@ -70,7 +70,7 @@ class NeoMpcPlanner : public nav2_core::Controller { * @param costmap_ros Costmap2DROS object of environment */ void configure( - const nav2::LifecycleNode::WeakPtr & parent, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr tf, const std::shared_ptr costmap_ros) override; @@ -144,12 +144,13 @@ class NeoMpcPlanner : public nav2_core::Controller { nav2_costmap_2d::Costmap2D * costmap_; rclcpp::Logger logger_ {rclcpp::get_logger("MPC")}; rclcpp::Clock::SharedPtr clock_; - nav2::Publisher::SharedPtr global_path_pub_; - nav2::LifecycleNode::WeakPtr node_; + std::shared_ptr> global_path_pub_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; tf2::Duration transform_tolerance_; - nav2::ServiceClient::SharedPtr client; + rclcpp::Client::SharedPtr client; - nav2::Publisher::SharedPtr carrot_pub_; + std::shared_ptr> + carrot_pub_; std::unique_ptr createCarrotMsg( const geometry_msgs::msg::PoseStamped & carrot_pose); diff --git a/src/NeoMpcPlanner.cpp b/src/NeoMpcPlanner.cpp index e8efb01..fe25a11 100755 --- a/src/NeoMpcPlanner.cpp +++ b/src/NeoMpcPlanner.cpp @@ -25,7 +25,7 @@ SOFTWARE. #include "../include/NeoMpcPlanner.h" #include -#include "nav2_ros_common/node_utils.hpp" +#include "nav2_util/node_utils.hpp" #include #include #include @@ -43,7 +43,7 @@ using std::hypot; using std::min; using std::max; using std::abs; -using nav2::declare_parameter_if_not_declared; +using nav2_util::declare_parameter_if_not_declared; using nav2_util::geometry_utils::euclidean_distance; using namespace nav2_costmap_2d; // NOLINT using rcl_interfaces::msg::ParameterType; @@ -245,8 +245,8 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( request->control_interval = 1.0 / control_frequency; // Use nav2::ServiceClient API - auto future_result = client->async_call(request); - auto out = future_result.get(); + auto result = client->async_send_request(request); + auto out = result.get(); geometry_msgs::msg::TwistStamped cmd_vel_final; cmd_vel_final = out->output_vel; @@ -287,7 +287,7 @@ void NeoMpcPlanner::setSpeedLimit( } void NeoMpcPlanner::configure( - const nav2::LifecycleNode::WeakPtr & parent, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr tf, const std::shared_ptr costmap_ros) { @@ -307,11 +307,11 @@ void NeoMpcPlanner::configure( client = node->create_client("optimizer"); global_path_pub_ = node->create_publisher("received_global_plan", 1); - nav2::declare_parameter_if_not_declared( + declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_dist_min", rclcpp::ParameterValue(0.5)); - nav2::declare_parameter_if_not_declared( + declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_dist_max", rclcpp::ParameterValue(0.5)); - nav2::declare_parameter_if_not_declared( + declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_dist_close_to_goal", rclcpp::ParameterValue(0.5)); node->get_parameter(plugin_name_ + ".lookahead_dist_min", lookahead_dist_min_); From 5e86c20e64393dd1c330cb05a4d1d31fd42a90ca Mon Sep 17 00:00:00 2001 From: Pradheep Date: Wed, 3 Dec 2025 14:01:44 +0100 Subject: [PATCH 02/13] adding a second lookahead point with a vector calculation for determining when the robot should turn --- include/NeoMpcPlanner.h | 1 + neo_mpc_planner2/mpc_optimization_server.py | 4 +++- src/NeoMpcPlanner.cpp | 8 ++++++++ 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/include/NeoMpcPlanner.h b/include/NeoMpcPlanner.h index 8f54ab2..1201101 100644 --- a/include/NeoMpcPlanner.h +++ b/include/NeoMpcPlanner.h @@ -165,6 +165,7 @@ class NeoMpcPlanner : public nav2_core::Controller { double lookahead_dist_min_ = 0.0; double lookahead_dist_max_ = 0.0; double lookahead_dist_close_to_goal_ = 0.0; + double lookahead_dist2 = 0.8; double control_frequency = 0.0; std::unique_ptr> diff --git a/neo_mpc_planner2/mpc_optimization_server.py b/neo_mpc_planner2/mpc_optimization_server.py index e45b3d2..3d412d4 100755 --- a/neo_mpc_planner2/mpc_optimization_server.py +++ b/neo_mpc_planner2/mpc_optimization_server.py @@ -121,6 +121,7 @@ def __init__(self): self.update_y = 0.0 self.update_yaw = 0.0 self.size_x_ = 0 + self.turn_yaw_ = 0.0 self.bnds = list() self.cons = [] @@ -248,7 +249,7 @@ def objective(self, cmd_vel): # i) Evaluvating cost for error in displacement and orientation step_dist_error = np.linalg.norm(curr_pos - np.array((self.x, self.y))) - step_orient_error = target_yaw - self.z + step_orient_error = self.turn_yaw_ - self.z self.cost_total += ((self.w_trans * step_dist_error**2) + (self.w_orient * step_orient_error**2)) / self.no_ctrl_steps self.cost_total += self.w_control * (np.linalg.norm(np.array((self.current_velocity.linear.x , self.current_velocity.linear.y, \ self.current_velocity.angular.z )) - np.array((cmd_vel[0+3*i], cmd_vel[1+3*i], cmd_vel[2+3*i])))) / self.no_ctrl_steps @@ -353,6 +354,7 @@ def optimizer(self, request, response): self.goal_pose = request.goal_pose self.update_opt_param = request.switch_opt self.control_interval = request.control_interval + self.turn_yaw_ = request.turn_yaw # on new goal reset all the flags and initializers if (self.old_goal != self.goal_pose): diff --git a/src/NeoMpcPlanner.cpp b/src/NeoMpcPlanner.cpp index fe25a11..7332e01 100755 --- a/src/NeoMpcPlanner.cpp +++ b/src/NeoMpcPlanner.cpp @@ -212,6 +212,9 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( // For now just for testing auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + // carrot_pose2 for determining the turn + auto carrot_pose2 = getLookAheadPoint(lookahead_dist2, transformed_plan); + // check if the pose orientation and robot orientation greater than 180 // change the lookahead accordingly double footprint_cost = collision_checker_->footprintCostAtPose( @@ -230,6 +233,10 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( slow_down_ = false; } + double delta_x = carrot_pose2.pose.position.x - carrot_pose.pose.position.x; + double delta_y = carrot_pose2.pose.position.y - carrot_pose.pose.position.y; + double target_yaw = std::atan2(delta_y, delta_x); // The desired orientation + if (footprint_cost == 255) { throw nav2_core::ControllerException("MPC detected collision!"); } @@ -239,6 +246,7 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( auto request = std::make_shared(); request->current_vel = speed; request->carrot_pose = carrot_pose; + request->turn_yaw = target_yaw; request->goal_pose = goal_pose; request->current_pose = position; request->switch_opt = closer_to_goal; From 4817dd59d41d4f688856a1c0a3bb58aca536e6bc Mon Sep 17 00:00:00 2001 From: Pradheep Date: Wed, 3 Dec 2025 18:44:09 +0100 Subject: [PATCH 03/13] Minor fixes for the orientation switch --- include/NeoMpcPlanner.h | 1 + neo_mpc_planner2/mpc_optimization_server.py | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/include/NeoMpcPlanner.h b/include/NeoMpcPlanner.h index 1201101..d723ff6 100644 --- a/include/NeoMpcPlanner.h +++ b/include/NeoMpcPlanner.h @@ -165,6 +165,7 @@ class NeoMpcPlanner : public nav2_core::Controller { double lookahead_dist_min_ = 0.0; double lookahead_dist_max_ = 0.0; double lookahead_dist_close_to_goal_ = 0.0; + // should be the boundary of the local costmap. If the costmap is smaller, then the omnidirectional behavior should be adapted double lookahead_dist2 = 0.8; double control_frequency = 0.0; diff --git a/neo_mpc_planner2/mpc_optimization_server.py b/neo_mpc_planner2/mpc_optimization_server.py index 3d412d4..3b4bead 100755 --- a/neo_mpc_planner2/mpc_optimization_server.py +++ b/neo_mpc_planner2/mpc_optimization_server.py @@ -249,7 +249,12 @@ def objective(self, cmd_vel): # i) Evaluvating cost for error in displacement and orientation step_dist_error = np.linalg.norm(curr_pos - np.array((self.x, self.y))) - step_orient_error = self.turn_yaw_ - self.z + + if self.update_opt_param==False: + step_orient_error = self.turn_yaw_ - self.z + else: + step_orient_error = target_yaw - self.z + self.cost_total += ((self.w_trans * step_dist_error**2) + (self.w_orient * step_orient_error**2)) / self.no_ctrl_steps self.cost_total += self.w_control * (np.linalg.norm(np.array((self.current_velocity.linear.x , self.current_velocity.linear.y, \ self.current_velocity.angular.z )) - np.array((cmd_vel[0+3*i], cmd_vel[1+3*i], cmd_vel[2+3*i])))) / self.no_ctrl_steps From ca63c09b12be62e696ec970c5406a87626ff2169 Mon Sep 17 00:00:00 2001 From: Pradheep Date: Mon, 8 Dec 2025 13:52:24 +0100 Subject: [PATCH 04/13] chaning the footprint calculation, adding debugs --- include/NeoMpcPlanner.h | 3 + neo_mpc_planner2/mpc_optimization_server.py | 184 ++++++++++++++------ src/NeoMpcPlanner.cpp | 8 +- 3 files changed, 137 insertions(+), 58 deletions(-) diff --git a/include/NeoMpcPlanner.h b/include/NeoMpcPlanner.h index d723ff6..b4ee231 100644 --- a/include/NeoMpcPlanner.h +++ b/include/NeoMpcPlanner.h @@ -152,6 +152,9 @@ class NeoMpcPlanner : public nav2_core::Controller { std::shared_ptr> carrot_pub_; + std::shared_ptr> + carrot_pub2_; + std::unique_ptr createCarrotMsg( const geometry_msgs::msg::PoseStamped & carrot_pose); diff --git a/neo_mpc_planner2/mpc_optimization_server.py b/neo_mpc_planner2/mpc_optimization_server.py index 3b4bead..1a9384b 100755 --- a/neo_mpc_planner2/mpc_optimization_server.py +++ b/neo_mpc_planner2/mpc_optimization_server.py @@ -40,12 +40,14 @@ from tf2_ros.transform_listener import TransformListener from rcl_interfaces.msg import SetParametersResult from rclpy.parameter import Parameter +from geometry_msgs.msg import Point32 +import copy class MpcOptimizationServer(Node): def __init__(self): super().__init__('mpc_optimization_server') - # declare parameters + # [Keep all existing parameter declarations] self.declare_parameter('acc_x_limit', value = 0.5) self.declare_parameter('acc_y_limit', value = 0.5) self.declare_parameter('acc_theta_limit', value = 0.5) @@ -71,7 +73,6 @@ def __init__(self): self.declare_parameter('low_pass_gain', value = 0.5) self.declare_parameter('opt_tolerance', value = 1e-5) self.declare_parameter('prediction_horizon', value = 0.5) - # self.declare_parameter('control_horizon', value = 0.5) self.declare_parameter('control_steps', value = 3) # Get Parameters @@ -106,6 +107,10 @@ def __init__(self): self.add_on_set_parameters_callback(self.cb_params) self.PubRaysPath = self.create_publisher(Path, 'local_plan', 10) self.Pubfootprint = self.create_publisher(PolygonStamped, 'predicted_footprint', 10) + + # NEW: Publisher for footprint array visualization + self.PubFootprintArray = self.create_publisher(Path, 'predicted_footprints_debug', 10) + self.current_pose = Pose() self.carrot_pose = PoseStamped() self.goal_pose = PoseStamped() @@ -123,6 +128,11 @@ def __init__(self): self.size_x_ = 0 self.turn_yaw_ = 0.0 + # NEW: Storage for debug footprints + self.debug_footprints = [] + self.debug_costs = [] + self.debug_poses = [] + self.bnds = list() self.cons = [] b_x_vel = (self.min_vel_x, self.max_vel_x) @@ -135,36 +145,30 @@ def __init__(self): self.cons.append({'type': 'ineq', 'fun': partial(self.f_constraint, index = i)}) self.initial_guess = np.zeros(self.no_ctrl_steps * 3) - self.dt = self.prediction_horizon /self.no_ctrl_steps #time_interval_between_control_pts used in integration + self.dt = self.prediction_horizon /self.no_ctrl_steps self.last_time = 0.0 self.update_opt_param = False self.subscription_footprint = self.create_subscription( - PolygonStamped, - '/local_costmap/published_footprint', - self.footprint_callback, - 10) - self.subscription_footprint # prevent unused variable warning. + PolygonStamped, + '/local_costmap/published_footprint', + self.footprint_callback, + 10) + self.subscription_footprint self.old_goal = PoseStamped() self.no_acceleration_limit = False self.collision = False self.collision_footprint = False self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - self.control_interval = 0.0; + self.control_interval = 0.0 def footprint_callback(self, msg): - self.footprint = msg.polygon + self.footprint = msg def f_constraint(self, initial, index): return self.max_vel_trans - (np.sqrt((initial[0 + index * 3]) * (initial[0 + index * 3]) +(initial[1 + index * 3]) * (initial[1 + index * 3]))) def euler_from_quaternion(self, x, y, z, w): - """ - Convert a quaternion into euler angles (roll, pitch, yaw) - roll is rotation around x in radians (counterclockwise) - pitch is rotation around y in radians (counterclockwise) - yaw is rotation around z in radians (counterclockwise) - """ t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + y * y) roll_x = math.atan2(t0, t1) @@ -178,7 +182,7 @@ def euler_from_quaternion(self, x, y, z, w): t4 = +1.0 - 2.0 * (y * y + z * z) yaw_z = math.atan2(t3, t4) - return roll_x, pitch_y, yaw_z # in radians + return roll_x, pitch_y, yaw_z def quaternion_from_euler(self, roll, pitch, yaw): cy = math.cos(yaw * 0.5) @@ -203,29 +207,31 @@ def initial_guess_update(self, init_guess,guess): return init_guess def objective(self, cmd_vel): - - self.cost_total= 0 + self.cost_total = 0 self.x = 0.0 self.y = 0.0 self.z = 0.0 + # NEW: Clear debug storage at start of each optimization + self.debug_footprints = [] + self.debug_costs = [] + self.debug_poses = [] + _, _, target_yaw = self.euler_from_quaternion(self.carrot_pose.pose.orientation.x, self.carrot_pose.pose.orientation.y, self.carrot_pose.pose.orientation.z, self.carrot_pose.pose.orientation.w) _, _, final_yaw = self.euler_from_quaternion(self.goal_pose.orientation.x, self.goal_pose.orientation.y, self.goal_pose.orientation.z, self.goal_pose.orientation.w) _, _, odom_yaw = self.euler_from_quaternion(self.current_pose.pose.orientation.x, self.current_pose.pose.orientation.y, self.current_pose.pose.orientation.z, self.goal_pose.orientation.w) - count = 1.0 - tot_x = self.current_velocity.linear.x - tot_y = self.current_velocity.linear.y - tot_z = self.current_velocity.angular.z curr_pos = np.array((self.carrot_pose.pose.position.x,self.carrot_pose.pose.position.y)) pos_x = self.current_pose.pose.position.x pos_y = self.current_pose.pose.position.y - c_c = 0.0 - + + update_footprint = PolygonStamped() + update_footprint.header = self.footprint.header + update_footprint.polygon.points = [ + Point32(x=p.x, y=p.y, z=p.z) for p in self.footprint.polygon.points + ] for i in range((self.no_ctrl_steps)): self.costmap_cost = 0 - update_footprint = Polygon() - update_footprint.points = self.footprint.points # Update the position for the predicted velocity self.z += cmd_vel[2+3*i] * self.dt @@ -236,18 +242,7 @@ def objective(self, cmd_vel): pos_x += cmd_vel[0+3*i] *np.cos(odom_yaw) * self.dt - cmd_vel[1+3*i] * np.sin(odom_yaw) * self.dt pos_y += cmd_vel[0+3*i] *np.sin(odom_yaw) * self.dt + cmd_vel[1+3*i] * np.cos(odom_yaw) * self.dt - for j in range(0, len(self.footprint.points)): - a = self.footprint.points[j].x - b = self.footprint.points[j].y - update_footprint.points[j].x = self.x + self.footprint.points[j].x * np.cos(self.z) - self.footprint.points[j].y * np.sin(self.z) - update_footprint.points[j].y = self.y + self.footprint.points[j].x * np.sin(self.z) + self.footprint.points[j].y * np.cos(self.z) - self.footprint.points[j].x = a - self.footprint.points[j].y = b - - mx1, my1 = self.costmap_ros.getWorldToMap(pos_x, pos_y) - self.costmap_cost += self.costmap_ros.getCost(mx1, my1) ** 2 - - # i) Evaluvating cost for error in displacement and orientation + # i) Evaluating cost for error in displacement and orientation step_dist_error = np.linalg.norm(curr_pos - np.array((self.x, self.y))) if self.update_opt_param==False: @@ -257,23 +252,100 @@ def objective(self, cmd_vel): self.cost_total += ((self.w_trans * step_dist_error**2) + (self.w_orient * step_orient_error**2)) / self.no_ctrl_steps self.cost_total += self.w_control * (np.linalg.norm(np.array((self.current_velocity.linear.x , self.current_velocity.linear.y, \ - self.current_velocity.angular.z )) - np.array((cmd_vel[0+3*i], cmd_vel[1+3*i], cmd_vel[2+3*i])))) / self.no_ctrl_steps - - # ii) Evaluvating obstacle cost - if(self.costmap_ros.getCost(mx1, my1) == 1.0): - self.cost_total += self.costmap_cost * 1000 / self.no_ctrl_steps - else: - self.cost_total += self.w_costmap_scale * self.costmap_cost / self.no_ctrl_steps - - if(self.costmap_ros.getFootprintCost(update_footprint) == 1.0): - self.cost_total += (self.costmap_ros.getFootprintCost(update_footprint)**2) * self.w_footprint_scale / self.no_ctrl_steps + self.current_velocity.angular.z )) - np.array((cmd_vel[0+3*i], cmd_vel[1+3*i], cmd_vel[2+3*i])))) / self.no_ctrl_steps + + # Transform footprint to WORLD frame using WORLD coordinates + for j in range(0, len(update_footprint.polygon.points)): + + # Transform to world frame using world position and orientation + update_footprint.polygon.points[j].x += (cmd_vel[0+3*i]*np.cos(self.z)* self.dt - cmd_vel[1+3*i]*np.sin(self.z)* self.dt) + update_footprint.polygon.points[j].y += (cmd_vel[0+3*i]*np.sin(self.z)* self.dt + cmd_vel[1+3*i]*np.cos(self.z)* self.dt) - # iii) terminal self.cost + mx1, my1 = self.costmap_ros.getWorldToMap(pos_x, pos_y) + self.footprint_cost = self.costmap_ros.getFootprintCost(update_footprint.polygon) + + # NEW: Store footprint and associated data for debugging + self.debug_footprints.append(copy.deepcopy(update_footprint)) + self.debug_costs.append(self.footprint_cost) + self.debug_poses.append({ + 'pos_x': pos_x, + 'pos_y': pos_y, + 'odom_yaw': odom_yaw, + 'step': i + }) + + # if (self.footprint_cost >= 0.5): + self.cost_total += self.footprint_cost + + # iii) terminal cost step_dist_error = np.linalg.norm(curr_pos - np.array((self.goal_pose.position.x,self.goal_pose.position.y))) step_orient_error = final_yaw - self.z - self.cost_total += ((self.w_trans * step_dist_error**2) + (self.w_orient* step_orient_error**2)) * self.w_terminal + self.cost_total += ((self.w_trans * step_dist_error**2) + (self.w_orient* step_orient_error**2)) * self.w_terminal + + + return self.cost_total + # NEW: Function to publish debug footprints + def publishDebugFootprints(self): + """ + Publishes the stored footprints as a Path for visualization in RViz. + Each footprint corner becomes a pose in the path. + """ + if not self.debug_footprints: + return + + debug_path = Path() + debug_path.header.stamp = self.get_clock().now().to_msg() + debug_path.header.frame_id = "map" + + print(len(self.debug_footprints)) + + # print("\n========== FOOTPRINT DEBUG INFO ==========") + # print(f"Total prediction steps: {len(self.debug_footprints)}") + + for i, (footprint, cost, pose_info) in enumerate(zip(self.debug_footprints, self.debug_costs, self.debug_poses)): + print(f"\nStep {i}:") + print(f" Position: ({pose_info['pos_x']:.3f}, {pose_info['pos_y']:.3f})") + print(f" Orientation: {pose_info['odom_yaw']:.3f} rad ({np.degrees(pose_info['odom_yaw']):.1f}°)") + print(f" Footprint Cost: {cost:.4f}") + + if cost >= 0.99: + print(f" ⚠️ COLLISION DETECTED!") + elif cost >= 0.7: + print(f" ⚠️ HIGH RISK ZONE") + elif cost >= 0.5: + print(f" ⚠️ WARNING ZONE") + + # Add each corner of the footprint as a pose + print(f" Footprint corners in world frame:") + for j, point in enumerate(footprint.polygon.points): + pose = PoseStamped() + pose.header.stamp = self.get_clock().now().to_msg() + pose.header.frame_id = "map" + pose.pose.position.x = point.x + pose.pose.position.y = point.y + pose.pose.position.z = 0.1 * i # Stack vertically for visualization + + # Use orientation to encode step number (for color coding in RViz) + q = self.quaternion_from_euler(0, 0, pose_info['odom_yaw']) + pose.pose.orientation.w = q[0] + pose.pose.orientation.x = q[1] + pose.pose.orientation.y = q[2] + pose.pose.orientation.z = q[3] + + debug_path.poses.append(pose) + + print(f" Corner {j}: ({point.x:.3f}, {point.y:.3f})") + + print("\n==========================================\n") + + self.PubFootprintArray.publish(debug_path) + + # print("\n==========================================\n") + + self.Pubfootprint.publish(footprint) + def publishLocalPlan(self, x): self.local_plan.poses.clear() try: @@ -297,6 +369,7 @@ def publishLocalPlan(self, x): self.local_plan.poses.append(pose) for i in range((self.no_ctrl_steps)): + print("step ", i, x[3*i], x[1 + 3*i]) pose = PoseStamped() yaw += x[2+3*i] * self.dt pos_x += x[3*i]*np.cos(yaw) * self.dt - x[1+3*i]*np.sin(yaw) * self.dt @@ -369,6 +442,10 @@ def optimizer(self, request, response): x = minimize(self.objective, self.initial_guess, method='SLSQP',bounds= self.bnds, constraints = self.cons, options={'ftol':self.opt_tolerance,'disp':False}) + + # NEW: Publish debug footprints after optimization + self.publishDebugFootprints() + self.publishLocalPlan(x.x) for i in range(0,3): x.x[i] = x.x[i] * self.low_pass_gain + self.last_control[i] * (1 - self.low_pass_gain) @@ -376,19 +453,16 @@ def optimizer(self, request, response): current_time = time.time() delta_t = current_time - self.last_time self.last_time = current_time - self.collision_check(x.x) if (self.collision == True or self.collision_footprint == True): response.output_vel.twist.linear.x = 0.0 response.output_vel.twist.linear.y = 0.0 response.output_vel.twist.angular.z = 0.0 self.waiting_time += delta_t - # After waiting for 3 seconds for the obstacle to clear, the robot proceeds further in the next service call. if (self.waiting_time >= 3.0): self.collision = False self.waiting_time = 0.0 else: - # avoiding sudden jerks and inertia temp_x = np.fmin(x.x[0], self.last_control[0] + self.acc_x_limit * self.control_interval) temp_y = np.fmin(x.x[1], self.last_control[1] + self.acc_y_limit * self.control_interval) temp_z = np.fmin(x.x[2], self.last_control[2] + self.acc_theta_limit * self.control_interval) @@ -451,4 +525,4 @@ def main(args=None): rclpy.spin(MpcOptimization) if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/src/NeoMpcPlanner.cpp b/src/NeoMpcPlanner.cpp index 7332e01..7244343 100755 --- a/src/NeoMpcPlanner.cpp +++ b/src/NeoMpcPlanner.cpp @@ -233,15 +233,15 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( slow_down_ = false; } - double delta_x = carrot_pose2.pose.position.x - carrot_pose.pose.position.x; - double delta_y = carrot_pose2.pose.position.y - carrot_pose.pose.position.y; - double target_yaw = std::atan2(delta_y, delta_x); // The desired orientation + double target_yaw = std::atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); // The desired orientation + double target_yaw_l1 = std::atan2(carrot_pose2.pose.position.y, carrot_pose2.pose.position.x); // Orientation for farther lookahead if (footprint_cost == 255) { throw nav2_core::ControllerException("MPC detected collision!"); } carrot_pub_->publish(createCarrotMsg(carrot_pose)); + carrot_pub2_->publish(createCarrotMsg(carrot_pose2)); auto request = std::make_shared(); request->current_vel = speed; @@ -269,6 +269,7 @@ void NeoMpcPlanner::activate() { global_path_pub_->on_activate(); carrot_pub_->on_activate(); + carrot_pub2_->on_activate(); auto node = node_.lock(); dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind(&NeoMpcPlanner::dynamicParametersCallback, this, std::placeholders::_1)); @@ -336,6 +337,7 @@ void NeoMpcPlanner::configure( RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); } carrot_pub_ = node->create_publisher("/lookahead_point", 1); + carrot_pub2_ = node->create_publisher("/lookahead_point2", 1); collision_checker_ = std::make_unique>(costmap_); } From e1dab53523180d2719b196ab201979d25f2656fd Mon Sep 17 00:00:00 2001 From: Pradheep Date: Mon, 15 Dec 2025 13:12:02 +0100 Subject: [PATCH 05/13] further improvements for handling narrower spaces --- neo_mpc_planner2/mpc_optimization_server.py | 122 ++++++++++++-------- src/NeoMpcPlanner.cpp | 22 ++++ 2 files changed, 97 insertions(+), 47 deletions(-) diff --git a/neo_mpc_planner2/mpc_optimization_server.py b/neo_mpc_planner2/mpc_optimization_server.py index 1a9384b..4f241be 100755 --- a/neo_mpc_planner2/mpc_optimization_server.py +++ b/neo_mpc_planner2/mpc_optimization_server.py @@ -42,6 +42,7 @@ from rclpy.parameter import Parameter from geometry_msgs.msg import Point32 import copy +from angles import shortest_angular_distance class MpcOptimizationServer(Node): def __init__(self): @@ -74,6 +75,8 @@ def __init__(self): self.declare_parameter('opt_tolerance', value = 1e-5) self.declare_parameter('prediction_horizon', value = 0.5) self.declare_parameter('control_steps', value = 3) + self.declare_parameter('sharp_turn_threshold', value = 0.52) + self.declare_parameter('tight_lookahead_dist_threshold', value = 0.5) # Get Parameters self.acc_x_limit = self.get_parameter('acc_x_limit').value @@ -102,6 +105,8 @@ def __init__(self): self.prediction_horizon= self.get_parameter('prediction_horizon').value self.no_ctrl_steps = self.get_parameter('control_steps').value self.waiting_time = self.get_parameter('waiting_time').value + self.sharp_turn_threshold = self.get_parameter('sharp_turn_threshold').value + self.tight_lookahead_dist_threshold = self.get_parameter('tight_lookahead_dist_threshold').value self.srv = self.create_service(Optimizer, 'optimizer', self.optimizer) self.add_on_set_parameters_callback(self.cb_params) @@ -119,6 +124,10 @@ def __init__(self): self.cost_total = 0.0 self.costmap_cost = 0.0 + self.turn_yaw_ = 0.0 + self.turn_yaw_tight_ = 0.0 + self.effective_turn_angle_ = 0.0 + self.carrot_pose_tight = PoseStamped() self.last_control = [0,0,0] self.costmap_ros = Costmap2d(self) @@ -142,7 +151,9 @@ def __init__(self): self.bnds.append(b_x_vel) self.bnds.append(b_y_vel) self.bnds.append(b_rot) + # Velocity magnitude constraint self.cons.append({'type': 'ineq', 'fun': partial(self.f_constraint, index = i)}) + # CBF is now a soft constraint (penalty in objective function) self.initial_guess = np.zeros(self.no_ctrl_steps * 3) self.dt = self.prediction_horizon /self.no_ctrl_steps @@ -206,6 +217,8 @@ def initial_guess_update(self, init_guess,guess): init_guess[0+3*(self.no_ctrl_steps-1):3+3*(self.no_ctrl_steps-1)] = guess[0:3] return init_guess + + # objective function: Distance calculation should be all done in the local frame of the robot def objective(self, cmd_vel): self.cost_total = 0 self.x = 0.0 @@ -221,7 +234,6 @@ def objective(self, cmd_vel): _, _, final_yaw = self.euler_from_quaternion(self.goal_pose.orientation.x, self.goal_pose.orientation.y, self.goal_pose.orientation.z, self.goal_pose.orientation.w) _, _, odom_yaw = self.euler_from_quaternion(self.current_pose.pose.orientation.x, self.current_pose.pose.orientation.y, self.current_pose.pose.orientation.z, self.goal_pose.orientation.w) - curr_pos = np.array((self.carrot_pose.pose.position.x,self.carrot_pose.pose.position.y)) pos_x = self.current_pose.pose.position.x pos_y = self.current_pose.pose.position.y @@ -242,17 +254,67 @@ def objective(self, cmd_vel): pos_x += cmd_vel[0+3*i] *np.cos(odom_yaw) * self.dt - cmd_vel[1+3*i] * np.sin(odom_yaw) * self.dt pos_y += cmd_vel[0+3*i] *np.sin(odom_yaw) * self.dt + cmd_vel[1+3*i] * np.cos(odom_yaw) * self.dt + # Check distance to nearest obstacle for adaptive behavior + # Use center point for this general context switch + mx_curr, my_curr = self.costmap_ros.getWorldToMap(pos_x, pos_y) + dist_to_obs = self.costmap_ros.esdf.get_distance_bilinear(mx_curr, my_curr) + + # Context-aware lookahead selection + # Use tight lookahead ONLY when: + # 1. Close to obstacles (< threshold) AND + # 2. Making a sharp turn (> sharp_turn_threshold) + use_tight_lookahead = (dist_to_obs < self.tight_lookahead_dist_threshold and + self.effective_turn_angle_ > self.sharp_turn_threshold) + + if use_tight_lookahead: + # Use tight lookahead for precise control near obstacles during turns + step_target_yaw = self.turn_yaw_tight_ + curr_pos = np.array((self.carrot_pose_tight.pose.position.x, self.carrot_pose_tight.pose.position.y)) + else: + # Use normal lookahead for smoother motion + step_target_yaw = self.turn_yaw_ + curr_pos = np.array((self.carrot_pose.pose.position.x, self.carrot_pose.pose.position.y)) + # i) Evaluating cost for error in displacement and orientation step_dist_error = np.linalg.norm(curr_pos - np.array((self.x, self.y))) - if self.update_opt_param==False: - step_orient_error = self.turn_yaw_ - self.z - else: - step_orient_error = target_yaw - self.z + # Adaptive Orientation Control + # Only use bidirectional alignment if: + # 1. Close to obstacles AND + # 2. Far enough from goal to avoid oscillations + if dist_to_obs < self.tight_lookahead_dist_threshold and step_dist_error > 0.2: + # Narrow space: Bidirectional Alignment + # Both self.turn_yaw_ and self.z are in local frame (relative to base_link) + # step_target_yaw is the angle to goal (0 = straight ahead) + + # Calculate both alignment options + forward_target = step_target_yaw + # For backward, add pi to flip 180 degrees + backward_target = step_target_yaw + np.pi + + # Calculate angular distance from current orientation (self.z) to each target + # Use ROS angles library for proper angle wrapping + diff_to_forward = abs(shortest_angular_distance(self.z, forward_target)) + diff_to_backward = abs(shortest_angular_distance(self.z, backward_target)) + + # Choose the alignment that requires less rotation + if diff_to_backward < diff_to_forward: + # Backward alignment is closer + print("Backward alignment is closer") + step_target_yaw = backward_target + else: + # Forward alignment is closer + print("Forward alignment is closer") + step_target_yaw = forward_target + + # Calculate angular error (handling wrapping) + # Use shortest_angular_distance to ensure we always take the shortest rotation + step_orient_error = shortest_angular_distance(self.z, step_target_yaw) + print("step_orient_error: ", step_orient_error) self.cost_total += ((self.w_trans * step_dist_error**2) + (self.w_orient * step_orient_error**2)) / self.no_ctrl_steps self.cost_total += self.w_control * (np.linalg.norm(np.array((self.current_velocity.linear.x , self.current_velocity.linear.y, \ - self.current_velocity.angular.z )) - np.array((cmd_vel[0+3*i], cmd_vel[1+3*i], cmd_vel[2+3*i])))) / self.no_ctrl_steps + self.current_velocity.angular.z )) - np.array((cmd_vel[0+3*i], cmd_vel[1+3*i], cmd_vel[2+3*i])))) / self.no_ctrl_steps # Transform footprint to WORLD frame using WORLD coordinates for j in range(0, len(update_footprint.polygon.points)): @@ -264,25 +326,13 @@ def objective(self, cmd_vel): mx1, my1 = self.costmap_ros.getWorldToMap(pos_x, pos_y) self.footprint_cost = self.costmap_ros.getFootprintCost(update_footprint.polygon) - # NEW: Store footprint and associated data for debugging - self.debug_footprints.append(copy.deepcopy(update_footprint)) - self.debug_costs.append(self.footprint_cost) - self.debug_poses.append({ - 'pos_x': pos_x, - 'pos_y': pos_y, - 'odom_yaw': odom_yaw, - 'step': i - }) - - # if (self.footprint_cost >= 0.5): - self.cost_total += self.footprint_cost + # Footprint collision cost + # self.cost_total += self.footprint_cost # iii) terminal cost step_dist_error = np.linalg.norm(curr_pos - np.array((self.goal_pose.position.x,self.goal_pose.position.y))) - step_orient_error = final_yaw - self.z - self.cost_total += ((self.w_trans * step_dist_error**2) + (self.w_orient* step_orient_error**2)) * self.w_terminal - - + # step_orient_error = final_yaw - self.z + self.cost_total += ((self.w_trans * step_dist_error**2)) * self.w_terminal return self.cost_total @@ -298,27 +348,9 @@ def publishDebugFootprints(self): debug_path = Path() debug_path.header.stamp = self.get_clock().now().to_msg() debug_path.header.frame_id = "map" - - print(len(self.debug_footprints)) - - # print("\n========== FOOTPRINT DEBUG INFO ==========") - # print(f"Total prediction steps: {len(self.debug_footprints)}") for i, (footprint, cost, pose_info) in enumerate(zip(self.debug_footprints, self.debug_costs, self.debug_poses)): - print(f"\nStep {i}:") - print(f" Position: ({pose_info['pos_x']:.3f}, {pose_info['pos_y']:.3f})") - print(f" Orientation: {pose_info['odom_yaw']:.3f} rad ({np.degrees(pose_info['odom_yaw']):.1f}°)") - print(f" Footprint Cost: {cost:.4f}") - - if cost >= 0.99: - print(f" ⚠️ COLLISION DETECTED!") - elif cost >= 0.7: - print(f" ⚠️ HIGH RISK ZONE") - elif cost >= 0.5: - print(f" ⚠️ WARNING ZONE") - # Add each corner of the footprint as a pose - print(f" Footprint corners in world frame:") for j, point in enumerate(footprint.polygon.points): pose = PoseStamped() pose.header.stamp = self.get_clock().now().to_msg() @@ -335,15 +367,9 @@ def publishDebugFootprints(self): pose.pose.orientation.z = q[3] debug_path.poses.append(pose) - - print(f" Corner {j}: ({point.x:.3f}, {point.y:.3f})") - - print("\n==========================================\n") self.PubFootprintArray.publish(debug_path) - # print("\n==========================================\n") - self.Pubfootprint.publish(footprint) def publishLocalPlan(self, x): @@ -369,7 +395,6 @@ def publishLocalPlan(self, x): self.local_plan.poses.append(pose) for i in range((self.no_ctrl_steps)): - print("step ", i, x[3*i], x[1 + 3*i]) pose = PoseStamped() yaw += x[2+3*i] * self.dt pos_x += x[3*i]*np.cos(yaw) * self.dt - x[1+3*i]*np.sin(yaw) * self.dt @@ -428,11 +453,14 @@ def collision_check(self, x): def optimizer(self, request, response): self.current_pose = request.current_pose self.carrot_pose = request.carrot_pose + self.carrot_pose_tight = request.carrot_pose_tight self.current_velocity = request.current_vel self.goal_pose = request.goal_pose self.update_opt_param = request.switch_opt self.control_interval = request.control_interval self.turn_yaw_ = request.turn_yaw + self.turn_yaw_tight_ = request.turn_yaw_tight + self.effective_turn_angle_ = request.effective_turn_angle # on new goal reset all the flags and initializers if (self.old_goal != self.goal_pose): diff --git a/src/NeoMpcPlanner.cpp b/src/NeoMpcPlanner.cpp index 7244343..83ab3bf 100755 --- a/src/NeoMpcPlanner.cpp +++ b/src/NeoMpcPlanner.cpp @@ -236,6 +236,25 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( double target_yaw = std::atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); // The desired orientation double target_yaw_l1 = std::atan2(carrot_pose2.pose.position.y, carrot_pose2.pose.position.x); // Orientation for farther lookahead + // Calculate effective turn angle for Python to use + // Use actual velocity to determine if robot is driving backward + bool driving_backward = (speed.linear.x < -0.05); // Threshold to avoid noise + + double effective_turn_angle; + if (driving_backward) { + // If driving backward, check turn angle relative to backward direction (±180°) + double backward_angle = std::abs(target_yaw) - M_PI; + effective_turn_angle = std::abs(backward_angle); + } else { + // If driving forward, use target_yaw directly + effective_turn_angle = std::abs(target_yaw); + } + + // Always calculate tight lookahead point for Python to choose from + const double TIGHT_LOOKAHEAD = 0.1; + auto carrot_pose_tight = getLookAheadPoint(TIGHT_LOOKAHEAD, transformed_plan); + double target_yaw_tight = std::atan2(carrot_pose_tight.pose.position.y, carrot_pose_tight.pose.position.x); + if (footprint_cost == 255) { throw nav2_core::ControllerException("MPC detected collision!"); } @@ -246,7 +265,10 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( auto request = std::make_shared(); request->current_vel = speed; request->carrot_pose = carrot_pose; + request->carrot_pose_tight = carrot_pose_tight; request->turn_yaw = target_yaw; + request->turn_yaw_tight = target_yaw_tight; + request->effective_turn_angle = effective_turn_angle; request->goal_pose = goal_pose; request->current_pose = position; request->switch_opt = closer_to_goal; From 7f9d8fe2f4254002f6b3fa8bc9777c859091249d Mon Sep 17 00:00:00 2001 From: Pradheep Date: Wed, 17 Dec 2025 11:48:47 +0100 Subject: [PATCH 06/13] feat - fixing terminal costs --- include/NeoMpcPlanner.h | 1 + neo_mpc_planner2/mpc_optimization_server.py | 41 +++++++++++---------- src/NeoMpcPlanner.cpp | 23 +++++++++++- 3 files changed, 44 insertions(+), 21 deletions(-) diff --git a/include/NeoMpcPlanner.h b/include/NeoMpcPlanner.h index b4ee231..0af0c69 100644 --- a/include/NeoMpcPlanner.h +++ b/include/NeoMpcPlanner.h @@ -168,6 +168,7 @@ class NeoMpcPlanner : public nav2_core::Controller { double lookahead_dist_min_ = 0.0; double lookahead_dist_max_ = 0.0; double lookahead_dist_close_to_goal_ = 0.0; + double tight_lookahead_dist = 0.1; // Distance for tight lookahead during turns // should be the boundary of the local costmap. If the costmap is smaller, then the omnidirectional behavior should be adapted double lookahead_dist2 = 0.8; double control_frequency = 0.0; diff --git a/neo_mpc_planner2/mpc_optimization_server.py b/neo_mpc_planner2/mpc_optimization_server.py index 4f241be..e7eed13 100755 --- a/neo_mpc_planner2/mpc_optimization_server.py +++ b/neo_mpc_planner2/mpc_optimization_server.py @@ -128,12 +128,12 @@ def __init__(self): self.turn_yaw_tight_ = 0.0 self.effective_turn_angle_ = 0.0 self.carrot_pose_tight = PoseStamped() + self.carrot_pose_terminal = PoseStamped() self.last_control = [0,0,0] self.costmap_ros = Costmap2d(self) self.update_x = 0.0 self.update_y = 0.0 - self.update_yaw = 0.0 self.size_x_ = 0 self.turn_yaw_ = 0.0 @@ -233,6 +233,7 @@ def objective(self, cmd_vel): _, _, target_yaw = self.euler_from_quaternion(self.carrot_pose.pose.orientation.x, self.carrot_pose.pose.orientation.y, self.carrot_pose.pose.orientation.z, self.carrot_pose.pose.orientation.w) _, _, final_yaw = self.euler_from_quaternion(self.goal_pose.orientation.x, self.goal_pose.orientation.y, self.goal_pose.orientation.z, self.goal_pose.orientation.w) _, _, odom_yaw = self.euler_from_quaternion(self.current_pose.pose.orientation.x, self.current_pose.pose.orientation.y, self.current_pose.pose.orientation.z, self.goal_pose.orientation.w) + _, _, terminal_yaw = self.euler_from_quaternion(self.carrot_pose_terminal.pose.orientation.x, self.carrot_pose_terminal.pose.orientation.y, self.carrot_pose_terminal.pose.orientation.z, self.carrot_pose_terminal.pose.orientation.w) pos_x = self.current_pose.pose.position.x pos_y = self.current_pose.pose.position.y @@ -266,7 +267,7 @@ def objective(self, cmd_vel): use_tight_lookahead = (dist_to_obs < self.tight_lookahead_dist_threshold and self.effective_turn_angle_ > self.sharp_turn_threshold) - if use_tight_lookahead: + if use_tight_lookahead and not self.update_opt_param: # Use tight lookahead for precise control near obstacles during turns step_target_yaw = self.turn_yaw_tight_ curr_pos = np.array((self.carrot_pose_tight.pose.position.x, self.carrot_pose_tight.pose.position.y)) @@ -279,18 +280,18 @@ def objective(self, cmd_vel): step_dist_error = np.linalg.norm(curr_pos - np.array((self.x, self.y))) # Adaptive Orientation Control - # Only use bidirectional alignment if: - # 1. Close to obstacles AND - # 2. Far enough from goal to avoid oscillations - if dist_to_obs < self.tight_lookahead_dist_threshold and step_dist_error > 0.2: + if dist_to_obs < self.tight_lookahead_dist_threshold and not self.update_opt_param: # Narrow space: Bidirectional Alignment # Both self.turn_yaw_ and self.z are in local frame (relative to base_link) - # step_target_yaw is the angle to goal (0 = straight ahead) + # self.turn_yaw_ is the angle to goal (0 = straight ahead) # Calculate both alignment options forward_target = step_target_yaw - # For backward, add pi to flip 180 degrees - backward_target = step_target_yaw + np.pi + # For backward, add/subtract pi to flip 180 degrees + if step_target_yaw >= 0: + backward_target = step_target_yaw - np.pi + else: + backward_target = step_target_yaw + np.pi # Calculate angular distance from current orientation (self.z) to each target # Use ROS angles library for proper angle wrapping @@ -300,17 +301,14 @@ def objective(self, cmd_vel): # Choose the alignment that requires less rotation if diff_to_backward < diff_to_forward: # Backward alignment is closer - print("Backward alignment is closer") step_target_yaw = backward_target else: # Forward alignment is closer - print("Forward alignment is closer") step_target_yaw = forward_target - + # Calculate angular error (handling wrapping) - # Use shortest_angular_distance to ensure we always take the shortest rotation step_orient_error = shortest_angular_distance(self.z, step_target_yaw) - print("step_orient_error: ", step_orient_error) + # step_orient_error = np.arctan2(np.sin(diff), np.cos(diff)) self.cost_total += ((self.w_trans * step_dist_error**2) + (self.w_orient * step_orient_error**2)) / self.no_ctrl_steps self.cost_total += self.w_control * (np.linalg.norm(np.array((self.current_velocity.linear.x , self.current_velocity.linear.y, \ @@ -323,16 +321,20 @@ def objective(self, cmd_vel): update_footprint.polygon.points[j].x += (cmd_vel[0+3*i]*np.cos(self.z)* self.dt - cmd_vel[1+3*i]*np.sin(self.z)* self.dt) update_footprint.polygon.points[j].y += (cmd_vel[0+3*i]*np.sin(self.z)* self.dt + cmd_vel[1+3*i]*np.cos(self.z)* self.dt) - mx1, my1 = self.costmap_ros.getWorldToMap(pos_x, pos_y) - self.footprint_cost = self.costmap_ros.getFootprintCost(update_footprint.polygon) + # mx1, my1 = self.costmap_ros.getWorldToMap(pos_x, pos_y) + # self.footprint_cost = self.costmap_ros.getFootprintCost(update_footprint.polygon) # Footprint collision cost # self.cost_total += self.footprint_cost # iii) terminal cost - step_dist_error = np.linalg.norm(curr_pos - np.array((self.goal_pose.position.x,self.goal_pose.position.y))) - # step_orient_error = final_yaw - self.z - self.cost_total += ((self.w_trans * step_dist_error**2)) * self.w_terminal + # Use carrot_pose_terminal (far lookahead) for terminal cost to avoid redundancy + # carrot_pose is already being tracked in step-by-step costs + terminal_goal_pos = np.array((self.carrot_pose_terminal.pose.position.x, self.carrot_pose_terminal.pose.position.y)) + step_dist_error = np.linalg.norm(terminal_goal_pos - np.array((self.x, self.y))) + step_orient_error = shortest_angular_distance(self.z, terminal_yaw) + + self.cost_total += ((self.w_trans * step_dist_error**2) + (self.w_orient * step_orient_error**2)) * self.w_terminal return self.cost_total @@ -454,6 +456,7 @@ def optimizer(self, request, response): self.current_pose = request.current_pose self.carrot_pose = request.carrot_pose self.carrot_pose_tight = request.carrot_pose_tight + self.carrot_pose_terminal = request.carrot_pose_terminal self.current_velocity = request.current_vel self.goal_pose = request.goal_pose self.update_opt_param = request.switch_opt diff --git a/src/NeoMpcPlanner.cpp b/src/NeoMpcPlanner.cpp index 83ab3bf..f419403 100755 --- a/src/NeoMpcPlanner.cpp +++ b/src/NeoMpcPlanner.cpp @@ -236,6 +236,11 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( double target_yaw = std::atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); // The desired orientation double target_yaw_l1 = std::atan2(carrot_pose2.pose.position.y, carrot_pose2.pose.position.x); // Orientation for farther lookahead + if (closer_to_goal) { + auto goal_orientation = transformed_plan.poses.back().pose.orientation; + target_yaw = createYawFromQuat(goal_orientation); + } + // Calculate effective turn angle for Python to use // Use actual velocity to determine if robot is driving backward bool driving_backward = (speed.linear.x < -0.05); // Threshold to avoid noise @@ -251,8 +256,7 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( } // Always calculate tight lookahead point for Python to choose from - const double TIGHT_LOOKAHEAD = 0.1; - auto carrot_pose_tight = getLookAheadPoint(TIGHT_LOOKAHEAD, transformed_plan); + auto carrot_pose_tight = getLookAheadPoint(tight_lookahead_dist, transformed_plan); double target_yaw_tight = std::atan2(carrot_pose_tight.pose.position.y, carrot_pose_tight.pose.position.x); if (footprint_cost == 255) { @@ -266,6 +270,7 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( request->current_vel = speed; request->carrot_pose = carrot_pose; request->carrot_pose_tight = carrot_pose_tight; + request->carrot_pose_terminal = carrot_pose2; // Use far lookahead for terminal cost request->turn_yaw = target_yaw; request->turn_yaw_tight = target_yaw_tight; request->effective_turn_angle = effective_turn_angle; @@ -344,13 +349,25 @@ void NeoMpcPlanner::configure( node, plugin_name_ + ".lookahead_dist_max", rclcpp::ParameterValue(0.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_dist_close_to_goal", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".tight_lookahead_dist", rclcpp::ParameterValue(0.1)); node->get_parameter(plugin_name_ + ".lookahead_dist_min", lookahead_dist_min_); node->get_parameter(plugin_name_ + ".lookahead_dist_max", lookahead_dist_max_); node->get_parameter( plugin_name_ + ".lookahead_dist_close_to_goal", lookahead_dist_close_to_goal_); + node->get_parameter(plugin_name_ + ".tight_lookahead_dist", tight_lookahead_dist); node->get_parameter("controller_frequency", control_frequency); + + // Validation: lookahead_dist_close_to_goal should be less than tight_lookahead_dist + if (lookahead_dist_close_to_goal_ >= tight_lookahead_dist) { + RCLCPP_WARN( + logger_, + "lookahead_dist_close_to_goal (%.2f) should be less than or equal to tight_lookahead_dist (%.2f)", + lookahead_dist_close_to_goal_, tight_lookahead_dist); + lookahead_dist_close_to_goal_ = tight_lookahead_dist; // Set to default value + } while (!client->wait_for_service(1s)) { if (!rclcpp::ok()) { @@ -397,6 +414,8 @@ NeoMpcPlanner::dynamicParametersCallback(std::vector paramete lookahead_dist_max_ = parameter.as_double(); } else if (name == plugin_name_ + "lookahead_dist_close_to_goal") { lookahead_dist_close_to_goal_ = parameter.as_double(); + } else if (name == plugin_name_ + "tight_lookahead_dist") { + tight_lookahead_dist = parameter.as_double(); } } mutex_.unlock(); From 78080c82a0ee7ad748b3906779a783d4933e212a Mon Sep 17 00:00:00 2001 From: Pradheep Date: Wed, 17 Dec 2025 12:07:02 +0100 Subject: [PATCH 07/13] update readme --- README.md | 54 +++++++++++++++++++++++++++++------------------------- 1 file changed, 29 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index 8950247..6b12510 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ neo_srvs2 ## Sample Parameters -``` +```yaml controller_server: ros__parameters: # controller server parameters (see Controller Server for more info) @@ -43,45 +43,49 @@ controller_server: stateful: True FollowPath: plugin: "neo_mpc_planner::NeoMpcPlanner" - lookahead_dist_min: 0.4 - lookahead_dist_max: 0.4 - lookahead_dist_close_to_goal: 0.4 + lookahead_dist_min: 0.4 # Minimum lookahead distance + lookahead_dist_max: 0.4 # Maximum lookahead distance + lookahead_dist_close_to_goal: 0.4 # Lookahead when close to goal (should be < tight_lookahead_dist) + tight_lookahead_dist: 0.1 # Reduced lookahead for sharp turns near obstacles control_steps: 3 mpc_optimization_server: ros__parameters: + # Acceleration limits acc_x_limit: 2.5 acc_y_limit: 2.5 acc_theta_limit: 3.0 + + # Velocity limits (min) min_vel_x: -0.7 min_vel_y: -0.7 min_vel_trans: -0.7 min_vel_theta: -0.7 + + # Velocity limits (max) max_vel_x: 0.7 max_vel_y: 0.7 max_vel_trans: 0.7 max_vel_theta: 0.7 - # Translation error weight - w_trans: 0.82 - # Orientation error weight - w_orient: 0.50 - # Control error weight - w_control: 0.05 - # Terminal weight - w_terminal: 0.05 - # Footprint weight - w_footprint: 0 - # Costmap weight - w_costmap: 0.05 - # Waiting time before the robot can try a maneuver, after it had been stuck in the obstacle - waiting_time: 3.0 - low_pass_gain: 0.5 - # Optimization tolerance, smaller it is, slower the performance - opt_tolerance: 1e-3 - # Time period, upto which MPC has to predict the control commands - prediction_horizon: 0.8 - # Number of steps that the prediction horizon needs to be splitted into - control_steps: 3 + + # Cost function weights + w_trans: 0.82 # Translation error weight + w_orient: 0.50 # Orientation error weight + w_control: 0.05 # Control effort weight + w_terminal: 0.05 # Terminal cost weight + w_footprint: 0 # Footprint collision weight (currently disabled) + w_costmap: 0.05 # Costmap traversal weight + + # Adaptive behavior parameters + sharp_turn_threshold: 0.52 # Turn angle threshold (radians) for tight lookahead activation (~30 degrees) + tight_lookahead_dist_threshold: 1.0 # Distance to obstacles (m) for adaptive orientation control + + # Optimizer parameters + waiting_time: 3.0 # Waiting time before retry after obstacle collision + low_pass_gain: 0.5 # Low-pass filter gain for velocity smoothing + opt_tolerance: 1e-3 # Optimization tolerance (smaller = slower but more accurate) + prediction_horizon: 0.8 # Time horizon for MPC prediction (seconds) + control_steps: 3 # Number of control steps to split prediction horizon ``` From 288d7cf98338903006c872ba80cb277af04b9f18 Mon Sep 17 00:00:00 2001 From: Pradheep Date: Fri, 9 Jan 2026 09:00:03 +0100 Subject: [PATCH 08/13] applying acceleration constraints to optimization --- neo_mpc_planner2/mpc_optimization_server.py | 41 ++++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) diff --git a/neo_mpc_planner2/mpc_optimization_server.py b/neo_mpc_planner2/mpc_optimization_server.py index e7eed13..8f94bda 100755 --- a/neo_mpc_planner2/mpc_optimization_server.py +++ b/neo_mpc_planner2/mpc_optimization_server.py @@ -151,9 +151,14 @@ def __init__(self): self.bnds.append(b_x_vel) self.bnds.append(b_y_vel) self.bnds.append(b_rot) + # Velocity magnitude constraint self.cons.append({'type': 'ineq', 'fun': partial(self.f_constraint, index = i)}) - # CBF is now a soft constraint (penalty in objective function) + + # Acceleration constraints for smooth motion + # self.cons.append({'type': 'ineq', 'fun': partial(self.acc_x_constraint, index = i)}) + # self.cons.append({'type': 'ineq', 'fun': partial(self.acc_y_constraint, index = i)}) + # self.cons.append({'type': 'ineq', 'fun': partial(self.acc_theta_constraint, index = i)}) self.initial_guess = np.zeros(self.no_ctrl_steps * 3) self.dt = self.prediction_horizon /self.no_ctrl_steps @@ -179,6 +184,40 @@ def footprint_callback(self, msg): def f_constraint(self, initial, index): return self.max_vel_trans - (np.sqrt((initial[0 + index * 3]) * (initial[0 + index * 3]) +(initial[1 + index * 3]) * (initial[1 + index * 3]))) + # Acceleration constraint functions + def acc_x_constraint(self, cmd_vel, index): + """Ensure vx doesn't exceed acceleration limits""" + if index == 0: + # First step: compare to last control + delta_v = cmd_vel[0] - self.last_control[0] + else: + # Subsequent steps: compare to previous step + delta_v = cmd_vel[0 + index * 3] - cmd_vel[0 + (index-1) * 3] + + max_delta = self.acc_x_limit * self.dt + # Return positive value when constraint is satisfied + return max_delta - abs(delta_v) + + def acc_y_constraint(self, cmd_vel, index): + """Ensure vy doesn't exceed acceleration limits""" + if index == 0: + delta_v = cmd_vel[1] - self.last_control[1] + else: + delta_v = cmd_vel[1 + index * 3] - cmd_vel[1 + (index-1) * 3] + + max_delta = self.acc_y_limit * self.dt + return max_delta - abs(delta_v) + + def acc_theta_constraint(self, cmd_vel, index): + """Ensure angular velocity doesn't exceed acceleration limits""" + if index == 0: + delta_v = cmd_vel[2] - self.last_control[2] + else: + delta_v = cmd_vel[2 + index * 3] - cmd_vel[2 + (index-1) * 3] + + max_delta = self.acc_theta_limit * self.dt + return max_delta - abs(delta_v) + def euler_from_quaternion(self, x, y, z, w): t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + y * y) From d17bd56905c9b9487d0f3e63848915c6cac7dddc Mon Sep 17 00:00:00 2001 From: Pradheep Date: Mon, 19 Jan 2026 11:11:54 +0100 Subject: [PATCH 09/13] updating the acceleration values and other related params in readme --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 6b12510..27f6d1d 100644 --- a/README.md +++ b/README.md @@ -52,9 +52,9 @@ controller_server: mpc_optimization_server: ros__parameters: # Acceleration limits - acc_x_limit: 2.5 - acc_y_limit: 2.5 - acc_theta_limit: 3.0 + acc_x_limit: 1.5 + acc_y_limit: 1.5 + acc_theta_limit: 0.8 # Velocity limits (min) min_vel_x: -0.7 @@ -77,8 +77,8 @@ mpc_optimization_server: w_costmap: 0.05 # Costmap traversal weight # Adaptive behavior parameters - sharp_turn_threshold: 0.52 # Turn angle threshold (radians) for tight lookahead activation (~30 degrees) - tight_lookahead_dist_threshold: 1.0 # Distance to obstacles (m) for adaptive orientation control + sharp_turn_threshold: 0.8 # Turn angle threshold (radians) for tight lookahead activation (~30 degrees) + tight_lookahead_dist_threshold: 0.5 # Distance to obstacles (m) for adaptive orientation control # Optimizer parameters waiting_time: 3.0 # Waiting time before retry after obstacle collision From af25d57c7303dc0802a69b2da05b31aa9e1d4c82 Mon Sep 17 00:00:00 2001 From: Pradheep Date: Sun, 25 Jan 2026 14:17:28 +0100 Subject: [PATCH 10/13] updating target yaw close to goal --- src/NeoMpcPlanner.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/NeoMpcPlanner.cpp b/src/NeoMpcPlanner.cpp index f419403..3d93680 100755 --- a/src/NeoMpcPlanner.cpp +++ b/src/NeoMpcPlanner.cpp @@ -259,6 +259,11 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( auto carrot_pose_tight = getLookAheadPoint(tight_lookahead_dist, transformed_plan); double target_yaw_tight = std::atan2(carrot_pose_tight.pose.position.y, carrot_pose_tight.pose.position.x); + if (closer_to_goal) { + auto goal_orientation = transformed_plan.poses.back().pose.orientation; + target_yaw_tight = createYawFromQuat(goal_orientation); + } + if (footprint_cost == 255) { throw nav2_core::ControllerException("MPC detected collision!"); } @@ -361,13 +366,13 @@ void NeoMpcPlanner::configure( node->get_parameter("controller_frequency", control_frequency); // Validation: lookahead_dist_close_to_goal should be less than tight_lookahead_dist - if (lookahead_dist_close_to_goal_ >= tight_lookahead_dist) { - RCLCPP_WARN( - logger_, - "lookahead_dist_close_to_goal (%.2f) should be less than or equal to tight_lookahead_dist (%.2f)", - lookahead_dist_close_to_goal_, tight_lookahead_dist); - lookahead_dist_close_to_goal_ = tight_lookahead_dist; // Set to default value - } + // if (lookahead_dist_close_to_goal_ >= tight_lookahead_dist) { + // RCLCPP_WARN( + // logger_, + // "lookahead_dist_close_to_goal (%.2f) should be less than or equal to tight_lookahead_dist (%.2f)", + // lookahead_dist_close_to_goal_, tight_lookahead_dist); + // lookahead_dist_close_to_goal_ = tight_lookahead_dist; // Set to default value + // } while (!client->wait_for_service(1s)) { if (!rclcpp::ok()) { From 5b3b2b572b77c59b2c6a96205fc606fa7a32994d Mon Sep 17 00:00:00 2001 From: Pradheep Date: Sun, 25 Jan 2026 14:18:02 +0100 Subject: [PATCH 11/13] updating the collision avoidance using footprint --- neo_mpc_planner2/mpc_optimization_server.py | 118 ++++++++++++-------- 1 file changed, 69 insertions(+), 49 deletions(-) diff --git a/neo_mpc_planner2/mpc_optimization_server.py b/neo_mpc_planner2/mpc_optimization_server.py index 8f94bda..36c0009 100755 --- a/neo_mpc_planner2/mpc_optimization_server.py +++ b/neo_mpc_planner2/mpc_optimization_server.py @@ -77,6 +77,7 @@ def __init__(self): self.declare_parameter('control_steps', value = 3) self.declare_parameter('sharp_turn_threshold', value = 0.52) self.declare_parameter('tight_lookahead_dist_threshold', value = 0.5) + self.declare_parameter('control_time_scale', value = 0.4) # Scale factor for collision check lookahead # Get Parameters self.acc_x_limit = self.get_parameter('acc_x_limit').value @@ -107,6 +108,7 @@ def __init__(self): self.waiting_time = self.get_parameter('waiting_time').value self.sharp_turn_threshold = self.get_parameter('sharp_turn_threshold').value self.tight_lookahead_dist_threshold = self.get_parameter('tight_lookahead_dist_threshold').value + self.control_time_scale = self.get_parameter('control_time_scale').value self.srv = self.create_service(Optimizer, 'optimizer', self.optimizer) self.add_on_set_parameters_callback(self.cb_params) @@ -378,40 +380,44 @@ def objective(self, cmd_vel): return self.cost_total # NEW: Function to publish debug footprints - def publishDebugFootprints(self): + def publishDebugFootprints(self, cmd_vel): """ Publishes the stored footprints as a Path for visualization in RViz. Each footprint corner becomes a pose in the path. """ - if not self.debug_footprints: - return + debug_footprints = [] + # add the next predicted footprint to the debug footprints + # use cmd_vel to calculate the updates to the footprint + update_footprint = copy.deepcopy(self.footprint) + _, _, odom_yaw = self.euler_from_quaternion(self.current_pose.pose.orientation.x, self.current_pose.pose.orientation.y, self.current_pose.pose.orientation.z, self.current_pose.pose.orientation.w) + next_odom_yaw = odom_yaw + cmd_vel[2] * self.control_time_scale * self.dt + for j in range(len(update_footprint.polygon.points)): + update_footprint.polygon.points[j].x += (cmd_vel[0]*np.cos(next_odom_yaw)* self.control_time_scale * self.dt - cmd_vel[1]*np.sin(next_odom_yaw)* self.control_time_scale * self.dt) + update_footprint.polygon.points[j].y += (cmd_vel[0]*np.sin(next_odom_yaw)* self.control_time_scale * self.dt + cmd_vel[1]*np.cos(next_odom_yaw)* self.control_time_scale * self.dt) + debug_footprints.append(copy.deepcopy(update_footprint)) debug_path = Path() debug_path.header.stamp = self.get_clock().now().to_msg() debug_path.header.frame_id = "map" - for i, (footprint, cost, pose_info) in enumerate(zip(self.debug_footprints, self.debug_costs, self.debug_poses)): - # Add each corner of the footprint as a pose - for j, point in enumerate(footprint.polygon.points): - pose = PoseStamped() - pose.header.stamp = self.get_clock().now().to_msg() - pose.header.frame_id = "map" - pose.pose.position.x = point.x - pose.pose.position.y = point.y - pose.pose.position.z = 0.1 * i # Stack vertically for visualization - - # Use orientation to encode step number (for color coding in RViz) - q = self.quaternion_from_euler(0, 0, pose_info['odom_yaw']) - pose.pose.orientation.w = q[0] - pose.pose.orientation.x = q[1] - pose.pose.orientation.y = q[2] - pose.pose.orientation.z = q[3] - - debug_path.poses.append(pose) + # Add each corner of the footprint as a pose + for j, point in enumerate(update_footprint.polygon.points): + pose = PoseStamped() + pose.header.stamp = self.get_clock().now().to_msg() + pose.header.frame_id = "map" + pose.pose.position.x = point.x + pose.pose.position.y = point.y + pose.pose.position.z = 0.1 # Stack vertically for visualization + + # Use orientation to encode step number (for color coding in RViz) + q = self.quaternion_from_euler(0, 0, self.z) + pose.pose.orientation.w = q[0] + pose.pose.orientation.x = q[1] + pose.pose.orientation.y = q[2] + pose.pose.orientation.z = q[3] + debug_path.poses.append(pose) self.PubFootprintArray.publish(debug_path) - - self.Pubfootprint.publish(footprint) def publishLocalPlan(self, x): self.local_plan.poses.clear() @@ -456,36 +462,47 @@ def publishLocalPlan(self, x): def collision_check(self, x): # Collision check with footprint - footprint = self.footprint pos_x = self.current_pose.pose.position.x pos_y = self.current_pose.pose.position.y _, _, odom_yaw = self.euler_from_quaternion(self.current_pose.pose.orientation.x, self.current_pose.pose.orientation.y, self.current_pose.pose.orientation.z, self.current_pose.pose.orientation.w) - pose = PoseStamped() - pose.pose.position.x = pos_x - pose.pose.position.y = pos_y - - for i in range((self.no_ctrl_steps)): - pose = PoseStamped() - odom_yaw += x[2+3*i] * self.dt - pos_x += x[3*i]*np.cos(odom_yaw) * self.dt - x[1+3*i]*np.sin(odom_yaw) * self.dt - pos_y += x[3*i]*np.sin(odom_yaw) * self.dt + x[1+3*i]*np.cos(odom_yaw) * self.dt - pose.pose.position.x = pos_x - pose.pose.position.y = pos_y - pose.header.stamp = self.get_clock().now().to_msg() - q = self.quaternion_from_euler(0, 0, odom_yaw) - mx1, my1 = self.costmap_ros.getWorldToMap(pos_x, pos_y) - col = self.costmap_ros.getCost(mx1, my1) - pose.pose.orientation.w = q[0] - pose.pose.orientation.x = q[1] - pose.pose.orientation.y = q[2] - pose.pose.orientation.z = q[3] - if (col >= 0.99): - self.collision = True - print("Collision ahead, stopping the robot") - break + # Calculate collision check lookahead time + # Scale dt to check at an intermediate point between control_interval and dt + # With scale=0.4 and dt=0.267s, this checks at ~0.11s (about 3 control cycles ahead) + collision_time = self.dt * self.control_time_scale + next_odom_yaw = odom_yaw + x[2] * collision_time + next_pos_x = pos_x + x[0]*np.cos(next_odom_yaw) * collision_time - x[1]*np.sin(next_odom_yaw) * collision_time + next_pos_y = pos_y + x[0]*np.sin(next_odom_yaw) * collision_time + x[1]*np.cos(next_odom_yaw) * collision_time + + # Check point collision at next predicted position + mx1, my1 = self.costmap_ros.getWorldToMap(next_pos_x, next_pos_y) + col = self.costmap_ros.getCost(mx1, my1) + if (col >= 0.99): + self.collision = True + print("Collision ahead, stopping the robot") + else: + self.collision = False - if (self.costmap_ros.getFootprintCost(footprint) == 1.0): + # Check footprint collision at NEXT predicted pose (not final) + # Transform footprint to the next predicted position + update_footprint = PolygonStamped() + update_footprint.header = self.footprint.header + update_footprint.polygon.points = [ + Point32(x=p.x, y=p.y, z=p.z) for p in self.footprint.polygon.points + ] + + for j in range(len(update_footprint.polygon.points)): + # Transform footprint polygon to next predicted pose + update_footprint.polygon.points[j].x = next_pos_x + ( + self.footprint.polygon.points[j].x * np.cos(next_odom_yaw) - + self.footprint.polygon.points[j].y * np.sin(next_odom_yaw) + ) + update_footprint.polygon.points[j].y = next_pos_y + ( + self.footprint.polygon.points[j].x * np.sin(next_odom_yaw) + + self.footprint.polygon.points[j].y * np.cos(next_odom_yaw) + ) + + if (self.costmap_ros.getFootprintCost(update_footprint.polygon) == 1.0): self.collision_footprint = True print("Footprint in collision, stopping the robot") else: @@ -514,7 +531,10 @@ def optimizer(self, request, response): method='SLSQP',bounds= self.bnds, constraints = self.cons, options={'ftol':self.opt_tolerance,'disp':False}) # NEW: Publish debug footprints after optimization - self.publishDebugFootprints() + self.publishDebugFootprints(x.x) + + # Check collision + # self.collision_check(x.x) self.publishLocalPlan(x.x) for i in range(0,3): From 7b9ed1221ad0fdeaf4047b94b676ff98fadf46a0 Mon Sep 17 00:00:00 2001 From: Pradheep Date: Sun, 25 Jan 2026 14:18:18 +0100 Subject: [PATCH 12/13] Adding updated parameters to readme --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 27f6d1d..f2b2974 100644 --- a/README.md +++ b/README.md @@ -45,15 +45,14 @@ controller_server: plugin: "neo_mpc_planner::NeoMpcPlanner" lookahead_dist_min: 0.4 # Minimum lookahead distance lookahead_dist_max: 0.4 # Maximum lookahead distance - lookahead_dist_close_to_goal: 0.4 # Lookahead when close to goal (should be < tight_lookahead_dist) tight_lookahead_dist: 0.1 # Reduced lookahead for sharp turns near obstacles control_steps: 3 mpc_optimization_server: ros__parameters: # Acceleration limits - acc_x_limit: 1.5 - acc_y_limit: 1.5 + acc_x_limit: 0.8 + acc_y_limit: 0.8 acc_theta_limit: 0.8 # Velocity limits (min) @@ -70,15 +69,16 @@ mpc_optimization_server: # Cost function weights w_trans: 0.82 # Translation error weight - w_orient: 0.50 # Orientation error weight + w_orient: 0.5 # Orientation error weight w_control: 0.05 # Control effort weight w_terminal: 0.05 # Terminal cost weight w_footprint: 0 # Footprint collision weight (currently disabled) w_costmap: 0.05 # Costmap traversal weight # Adaptive behavior parameters - sharp_turn_threshold: 0.8 # Turn angle threshold (radians) for tight lookahead activation (~30 degrees) + sharp_turn_threshold: 0.3 # Turn angle threshold (radians) for tight lookahead activation tight_lookahead_dist_threshold: 0.5 # Distance to obstacles (m) for adaptive orientation control + control_time_scale: 0.2 # Temporal scale factor (0.0-1.0, fraction of dt to match cmd_vel timing) # Optimizer parameters waiting_time: 3.0 # Waiting time before retry after obstacle collision From 176b1d8505cdebfb619697ec69994cd5c12ef68b Mon Sep 17 00:00:00 2001 From: Pradheep Date: Mon, 26 Jan 2026 06:26:41 +0100 Subject: [PATCH 13/13] adding collision avoidance for footprint --- neo_mpc_planner2/mpc_optimization_server.py | 25 +++++++-------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/neo_mpc_planner2/mpc_optimization_server.py b/neo_mpc_planner2/mpc_optimization_server.py index 36c0009..f0f0c42 100755 --- a/neo_mpc_planner2/mpc_optimization_server.py +++ b/neo_mpc_planner2/mpc_optimization_server.py @@ -483,24 +483,15 @@ def collision_check(self, x): else: self.collision = False - # Check footprint collision at NEXT predicted pose (not final) - # Transform footprint to the next predicted position - update_footprint = PolygonStamped() - update_footprint.header = self.footprint.header - update_footprint.polygon.points = [ - Point32(x=p.x, y=p.y, z=p.z) for p in self.footprint.polygon.points - ] - + # Check footprint collision at next predicted pose + # Footprint is already in map frame, offset by predicted displacement + update_footprint = copy.deepcopy(self.footprint) + # Calculate displacement in world frame + delta_x = x[0]*np.cos(odom_yaw) * collision_time - x[1]*np.sin(odom_yaw) * collision_time + delta_y = x[0]*np.sin(odom_yaw) * collision_time + x[1]*np.cos(odom_yaw) * collision_time for j in range(len(update_footprint.polygon.points)): - # Transform footprint polygon to next predicted pose - update_footprint.polygon.points[j].x = next_pos_x + ( - self.footprint.polygon.points[j].x * np.cos(next_odom_yaw) - - self.footprint.polygon.points[j].y * np.sin(next_odom_yaw) - ) - update_footprint.polygon.points[j].y = next_pos_y + ( - self.footprint.polygon.points[j].x * np.sin(next_odom_yaw) + - self.footprint.polygon.points[j].y * np.cos(next_odom_yaw) - ) + update_footprint.polygon.points[j].x += delta_x + update_footprint.polygon.points[j].y += delta_y if (self.costmap_ros.getFootprintCost(update_footprint.polygon) == 1.0): self.collision_footprint = True