Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix/controller exception #23

Merged
merged 2 commits into from
Nov 9, 2022
Merged

Fix/controller exception #23

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
722e4d4
fix for acceleration limits (#17)
padhupradheep Sep 15, 2022
d78379c
on par with the controller server
padhupradheep Nov 9, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions include/NeoMpcPlanner.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down
14 changes: 8 additions & 6 deletions neo_mpc_planner2/mpc_optimization_server.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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
Expand Down
14 changes: 8 additions & 6 deletions src/NeoMpcPlanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ SOFTWARE.
#include <vector>
#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"
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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));
Expand All @@ -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);

Expand Down Expand Up @@ -287,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;
Expand All @@ -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()) {
Expand Down