fix for acceleration limits (#17)

neobotix · Sep 15, 2022 · 2035608 · 2035608
1 parent aafc77d
commit 2035608
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 6 deletions.
diff --git 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<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
   collision_checker_;

diff --git 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
@@ -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()) {