From 47837800eb1000637c861d0e19ac2288c1a4ac5c Mon Sep 17 00:00:00 2001 From: DatSpace Date: Mon, 7 Jun 2021 11:49:04 +0200 Subject: [PATCH 1/2] Added navfn path length threshold --- navfn/include/navfn/navfn_ros.h | 2 +- navfn/src/navfn_ros.cpp | 17 ++++++++++++++++- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/navfn/include/navfn/navfn_ros.h b/navfn/include/navfn/navfn_ros.h index 9438007823..7f3a26452f 100644 --- a/navfn/include/navfn/navfn_ros.h +++ b/navfn/include/navfn/navfn_ros.h @@ -178,7 +178,7 @@ namespace navfn { void mapToWorld(double mx, double my, double& wx, double& wy); void clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my); - double planner_window_x_, planner_window_y_, default_tolerance_; + double planner_window_x_, planner_window_y_, default_tolerance_, plan_lenght_threshold_; boost::mutex mutex_; ros::ServiceServer make_plan_srv_; std::string global_frame_; diff --git a/navfn/src/navfn_ros.cpp b/navfn/src/navfn_ros.cpp index d78ca6d8a0..17fb3a9b53 100644 --- a/navfn/src/navfn_ros.cpp +++ b/navfn/src/navfn_ros.cpp @@ -80,6 +80,7 @@ namespace navfn { private_nh.param("planner_window_x", planner_window_x_, 0.0); private_nh.param("planner_window_y", planner_window_y_, 0.0); private_nh.param("default_tolerance", default_tolerance_, 0.0); + private_nh.param("plan_lenght_threshold", plan_lenght_threshold_, 50.0); make_plan_srv_ = private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this); @@ -296,7 +297,7 @@ namespace navfn { plan.push_back(goal_copy); } else{ - ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen."); + ROS_WARN("Failed to get a plan from potential when a legal potential was found. This is probably due to the plan lenght threshold."); } } @@ -407,11 +408,21 @@ namespace navfn { int len = planner_->getPathLen(); ros::Time plan_time = ros::Time::now(); + double prev_wx, prev_wy; + double path_len = 0; + for(int i = len - 1; i >= 0; --i){ //convert the plan to world coordinates double world_x, world_y; mapToWorld(x[i], y[i], world_x, world_y); + if (i < len - 1){ + path_len += sqrt(pow(world_x - prev_wx, 2.0) + pow(world_y - prev_wy, 2.0)); + } + + prev_wx = world_x; + prev_wy = world_y; + geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = global_frame_; @@ -424,6 +435,10 @@ namespace navfn { pose.pose.orientation.w = 1.0; plan.push_back(pose); } + + if (path_len > plan_lenght_threshold_){ + plan.clear(); + } //publish the plan for visualization purposes publishPlan(plan, 0.0, 1.0, 0.0, 0.0); From df474350b5d8d0c25436977a8bd32427b89a1b10 Mon Sep 17 00:00:00 2001 From: DatSpace Date: Fri, 11 Jun 2021 10:34:24 +0200 Subject: [PATCH 2/2] Fixed typo and set length check to disabled by default. --- navfn/include/navfn/navfn_ros.h | 2 +- navfn/src/navfn_ros.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/navfn/include/navfn/navfn_ros.h b/navfn/include/navfn/navfn_ros.h index 7f3a26452f..04fc56ec16 100644 --- a/navfn/include/navfn/navfn_ros.h +++ b/navfn/include/navfn/navfn_ros.h @@ -178,7 +178,7 @@ namespace navfn { void mapToWorld(double mx, double my, double& wx, double& wy); void clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my); - double planner_window_x_, planner_window_y_, default_tolerance_, plan_lenght_threshold_; + double planner_window_x_, planner_window_y_, default_tolerance_, plan_length_threshold_; boost::mutex mutex_; ros::ServiceServer make_plan_srv_; std::string global_frame_; diff --git a/navfn/src/navfn_ros.cpp b/navfn/src/navfn_ros.cpp index 17fb3a9b53..ec4a7d08a7 100644 --- a/navfn/src/navfn_ros.cpp +++ b/navfn/src/navfn_ros.cpp @@ -80,7 +80,7 @@ namespace navfn { private_nh.param("planner_window_x", planner_window_x_, 0.0); private_nh.param("planner_window_y", planner_window_y_, 0.0); private_nh.param("default_tolerance", default_tolerance_, 0.0); - private_nh.param("plan_lenght_threshold", plan_lenght_threshold_, 50.0); + private_nh.param("plan_length_threshold", plan_length_threshold_, 0.0); make_plan_srv_ = private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this); @@ -417,7 +417,7 @@ namespace navfn { mapToWorld(x[i], y[i], world_x, world_y); if (i < len - 1){ - path_len += sqrt(pow(world_x - prev_wx, 2.0) + pow(world_y - prev_wy, 2.0)); + path_len += hypot(world_x-prev_wx, world_y-prev_wy); } prev_wx = world_x; @@ -436,7 +436,7 @@ namespace navfn { plan.push_back(pose); } - if (path_len > plan_lenght_threshold_){ + if (plan_length_threshold_ > 0.0 && path_len > plan_length_threshold_){ plan.clear(); }