From 722e4d49e7172968e8f8afcdf23f47c06980ab1b Mon Sep 17 00:00:00 2001 From: Pradheep Krishna Date: Thu, 15 Sep 2022 12:25:48 +0200 Subject: [PATCH 1/2] fix for acceleration limits (#17) --- include/NeoMpcPlanner.h | 1 + neo_mpc_planner2/mpc_optimization_server.py | 14 ++++++++------ src/NeoMpcPlanner.cpp | 2 ++ 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/include/NeoMpcPlanner.h b/include/NeoMpcPlanner.h index f75e907..f465492 100644 --- a/include/NeoMpcPlanner.h +++ b/include/NeoMpcPlanner.h @@ -162,6 +162,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 control_frequency = 0.0; std::unique_ptr> collision_checker_; diff --git a/neo_mpc_planner2/mpc_optimization_server.py b/neo_mpc_planner2/mpc_optimization_server.py index c4bea7f..4b8992a 100755 --- a/neo_mpc_planner2/mpc_optimization_server.py +++ b/neo_mpc_planner2/mpc_optimization_server.py @@ -146,6 +146,7 @@ def __init__(self): self.collision_footprint = False self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) + self.control_interval = 0.0; def footprint_callback(self, msg): self.footprint = msg.polygon @@ -348,6 +349,7 @@ def optimizer(self, request, response): 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 # on new goal reset all the flags and initializers if (self.old_goal != self.goal_pose): @@ -377,13 +379,13 @@ def optimizer(self, request, response): self.waiting_time = 0.0 else: # avoiding sudden jerks and inertia - temp_x = np.sign(x.x[0]) * np.fmin(abs(x.x[0]), abs(self.last_control[0]) + self.acc_x_limit * self.dt) - temp_y = np.sign(x.x[1]) * np.fmin(abs(x.x[1]), abs(self.last_control[1]) + self.acc_y_limit * self.dt) - temp_z = np.sign(x.x[2]) * np.fmin(abs(x.x[2]), abs(self.last_control[2]) + self.acc_theta_limit * self.dt) + 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) - response.output_vel.twist.linear.x = np.sign(temp_x) * np.fmax(abs(temp_x), abs(self.last_control[0]) - self.acc_x_limit * self.dt) - response.output_vel.twist.linear.y = np.sign(temp_y) * np.fmax(abs(temp_y), abs(self.last_control[1]) - self.acc_y_limit * self.dt) - response.output_vel.twist.angular.z = np.sign(temp_z) * np.fmax(abs(temp_z), abs(self.last_control[2]) - self.acc_theta_limit * self.dt) + response.output_vel.twist.linear.x = np.fmax(temp_x, self.last_control[0] - self.acc_x_limit * self.control_interval) + response.output_vel.twist.linear.y = np.fmax(temp_y, self.last_control[1] - self.acc_y_limit * self.control_interval) + response.output_vel.twist.angular.z = np.fmax(temp_z, self.last_control[2] - self.acc_theta_limit * self.control_interval) self.last_control[0] = response.output_vel.twist.linear.x self.last_control[1] = response.output_vel.twist.linear.y diff --git a/src/NeoMpcPlanner.cpp b/src/NeoMpcPlanner.cpp index c3c07f8..2ca1203 100755 --- a/src/NeoMpcPlanner.cpp +++ b/src/NeoMpcPlanner.cpp @@ -242,6 +242,7 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( request->goal_pose = goal_pose; request->current_pose = position; request->switch_opt = closer_to_goal; + request->control_interval = 1.0 / control_frequency; auto result = client->async_send_request(request); @@ -311,6 +312,7 @@ void NeoMpcPlanner::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & p node->get_parameter( plugin_name_ + ".lookahead_dist_close_to_goal", lookahead_dist_close_to_goal_); + node->get_parameter("controller_frequency", control_frequency); while (!client->wait_for_service(1s)) { if (!rclcpp::ok()) { From d78379c1dd5cf229057f8825ffbbfe12f7c0c7ec Mon Sep 17 00:00:00 2001 From: Pradheep Date: Wed, 9 Nov 2022 16:32:45 +0100 Subject: [PATCH 2/2] on par with the controller server --- src/NeoMpcPlanner.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/NeoMpcPlanner.cpp b/src/NeoMpcPlanner.cpp index 2ca1203..dd1a62d 100755 --- a/src/NeoMpcPlanner.cpp +++ b/src/NeoMpcPlanner.cpp @@ -31,7 +31,7 @@ SOFTWARE. #include #include "nav2_util/line_iterator.hpp" #include "nav2_core/goal_checker.hpp" -#include "nav2_core/exceptions.hpp" +#include "nav2_core/controller_exceptions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -67,13 +67,13 @@ nav_msgs::msg::Path NeoMpcPlanner::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) { if (global_plan_.poses.empty()) { - throw nav2_core::PlannerException("Received plan with zero length"); + throw nav2_core::ControllerException("Received plan with zero length"); } // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { - throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); + throw nav2_core::ControllerException("Unable to transform robot pose into global plan's frame"); } // We'll discard points on the plan that are outside the local costmap @@ -128,7 +128,7 @@ nav_msgs::msg::Path NeoMpcPlanner::transformGlobalPlan( global_path_pub_->publish(transformed_plan); if (transformed_plan.poses.empty()) { - throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + throw nav2_core::ControllerException("Resulting plan has 0 poses in it."); } return transformed_plan; @@ -231,7 +231,7 @@ geometry_msgs::msg::TwistStamped NeoMpcPlanner::computeVelocityCommands( } if (footprint_cost == 255) { - throw nav2_core::PlannerException("MPC detected collision!"); + throw nav2_core::ControllerException("MPC detected collision!"); } carrot_pub_->publish(createCarrotMsg(carrot_pose)); @@ -288,7 +288,7 @@ void NeoMpcPlanner::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & p auto node = parent.lock(); node_ = parent; if (!node) { - throw nav2_core::PlannerException("Unable to lock node!"); + throw nav2_core::ControllerException("Unable to lock node!"); } costmap_ros_ = costmap_ros;