From a0854a3d1d089a40fb0ee69cdb9928d701295fdb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Wed, 30 Jan 2019 13:21:31 +0100 Subject: [PATCH 1/6] Call reset on new plan, not new goal The documentation on `reset()` says: > called at the beginning of every new navigation, i.e. when we get a > new global plan The `reset()` functions were called in `setGoalPose()`, i.e. when the user gives a new goal. However, when the local planner fails and the global planner replans, `setGoalPose()` is not called, but `setPlan()` is. This commit makes sure that `reset()` is also called in this case. --- dwb_local_planner/src/dwb_local_planner.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/dwb_local_planner/src/dwb_local_planner.cpp b/dwb_local_planner/src/dwb_local_planner.cpp index a87e8b9..d338ceb 100644 --- a/dwb_local_planner/src/dwb_local_planner.cpp +++ b/dwb_local_planner/src/dwb_local_planner.cpp @@ -161,18 +161,18 @@ void DWBLocalPlanner::setGoalPose(const nav_2d_msgs::Pose2DStamped& goal_pose) { ROS_INFO_NAMED("DWBLocalPlanner", "New Goal Received."); goal_pose_ = goal_pose; - traj_generator_->reset(); - goal_checker_->reset(); - for (TrajectoryCritic::Ptr critic : critics_) - { - critic->reset(); - } } void DWBLocalPlanner::setPlan(const nav_2d_msgs::Path2D& path) { pub_.publishGlobalPlan(path); global_plan_ = path; + traj_generator_->reset(); + goal_checker_->reset(); + for (TrajectoryCritic::Ptr critic : critics_) + { + critic->reset(); + } } nav_2d_msgs::Twist2DStamped DWBLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose, From 56896fea961025edf4b24bec8efb3588fcb42730 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 3 Jul 2020 12:55:25 +0200 Subject: [PATCH 2/6] Revert "Update reset documentation (#31)" This reverts commit 8248d6c67b6279e446cfa0c5c63bfeb728b386f6. --- dwb_local_planner/README.md | 2 +- dwb_local_planner/include/dwb_local_planner/trajectory_critic.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dwb_local_planner/README.md b/dwb_local_planner/README.md index 870ac46..4e9ff24 100644 --- a/dwb_local_planner/README.md +++ b/dwb_local_planner/README.md @@ -72,7 +72,7 @@ virtual bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geomet * `void initialize(const ros::NodeHandle& planner_nh, std::string name, nav_core2::Costmap::Ptr costmap)` - called once on startup, and then calls `onInit` * `void onInit()` - May be overwritten to load parameters as needed. - * `void reset()` - called at the beginning of every new navigation, i.e. when we get a new global goal via `setGoalPose`. + * `void reset()` - called at the beginning of every new navigation, i.e. when we get a new global plan. * `bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel, const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan)` - called once per iteration of the planner, prior to the evaluation of all the trajectories * `double scoreTrajectory(const dwb_msgs::Trajectory2D& traj)` - called once per trajectory * `void debrief(const nav_2d_msgs::Twist2D& cmd_vel)` - called after all the trajectories to notify what trajectory was chosen. diff --git a/dwb_local_planner/include/dwb_local_planner/trajectory_critic.h b/dwb_local_planner/include/dwb_local_planner/trajectory_critic.h index 55fb8ce..f74fb19 100644 --- a/dwb_local_planner/include/dwb_local_planner/trajectory_critic.h +++ b/dwb_local_planner/include/dwb_local_planner/trajectory_critic.h @@ -104,7 +104,7 @@ class TrajectoryCritic /** * @brief Reset the state of the critic * - * Reset is called when the planner receives a new global goal. + * Reset is called when the planner receives a new global plan. * This can be used to discard information specific to one plan. */ virtual void reset() {} From 865f9d57484f8ffc08f3b8cf1897dd6fc91c7682 Mon Sep 17 00:00:00 2001 From: Nils Niemann Date: Wed, 8 Jan 2020 09:22:32 +0100 Subject: [PATCH 3/6] Extended the dwb_local_planner with a split_path option. When split_path is set to true, upon receiving a new path from the global planner, dwb will now split the path into segments of the same movement direction (roughly forwards/backwards/in-place-rotation). The segments will be treated as if they were given by the global planner one by one. This avoids taking shortcuts during complex maneuvers. --- .../dwb_local_planner/dwb_local_planner.h | 15 +- dwb_local_planner/src/dwb_local_planner.cpp | 182 +++++++++++++++++- 2 files changed, 191 insertions(+), 6 deletions(-) diff --git a/dwb_local_planner/include/dwb_local_planner/dwb_local_planner.h b/dwb_local_planner/include/dwb_local_planner/dwb_local_planner.h index 8af480b..5199159 100644 --- a/dwb_local_planner/include/dwb_local_planner/dwb_local_planner.h +++ b/dwb_local_planner/include/dwb_local_planner/dwb_local_planner.h @@ -171,8 +171,21 @@ class DWBLocalPlanner: public nav_core2::LocalPlanner */ geometry_msgs::Pose2D transformPoseToLocal(const nav_2d_msgs::Pose2DStamped& pose); - nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan + + /** + * @brief Reset the critics and other plugins, e.g. when getting a new plan, + * or starting the next path segment. + */ + virtual void resetPlugins(); + + std::vector global_plan_segments_; ///< Path segments of same movement direction + // (forward/backward/in-place rotation) + nav_2d_msgs::Path2D global_plan_; ///< The currently active segment of the plan, or the whole plan if + // path segmentation is disabled. + nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose + nav_2d_msgs::Pose2DStamped intermediate_goal_pose_; ///< Goal of current path segment + bool prune_plan_; double prune_distance_; bool debug_trajectory_details_; diff --git a/dwb_local_planner/src/dwb_local_planner.cpp b/dwb_local_planner/src/dwb_local_planner.cpp index d338ceb..2ea5153 100644 --- a/dwb_local_planner/src/dwb_local_planner.cpp +++ b/dwb_local_planner/src/dwb_local_planner.cpp @@ -148,11 +148,37 @@ bool DWBLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, cons // Update time stamp of goal pose goal_pose_.header.stamp = pose.header.stamp; + intermediate_goal_pose_.header.stamp = pose.header.stamp; - bool ret = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), velocity); + // use the goal_checker_ to check if the intermediate goal was reached. + bool ret = goal_checker_->isGoalReached( + transformPoseToLocal(pose), + transformPoseToLocal(intermediate_goal_pose_), + velocity); if (ret) { - ROS_INFO_THROTTLE_NAMED(1.0, "DWBLocalPlanner", "Goal reached!"); + if (global_plan_segments_.empty()) + ROS_INFO_THROTTLE_NAMED(1.0, "DWBLocalPlanner", "Goal reached!"); + else + { + ROS_INFO_THROTTLE_NAMED(1.0, "DWBLocalPlanner", "Intermediate goal reached!"); + ret = false; // reset to false, as we only finished one segment. + + // activate next path segment + global_plan_ = global_plan_segments_[0]; + global_plan_segments_.erase(global_plan_segments_.begin()); + // set the next goal pose + intermediate_goal_pose_.header = global_plan_.header; + intermediate_goal_pose_.pose = global_plan_.poses.back(); + + // publish the next path segment + pub_.publishGlobalPlan(global_plan_); + + // reset critics etc, as we changed the global_plan_ + resetPlugins(); + // prepare(...) will be called soon, automatically, when computing the + // velocity commands. + } } return ret; } @@ -161,12 +187,157 @@ void DWBLocalPlanner::setGoalPose(const nav_2d_msgs::Pose2DStamped& goal_pose) { ROS_INFO_NAMED("DWBLocalPlanner", "New Goal Received."); goal_pose_ = goal_pose; + intermediate_goal_pose_ = goal_pose; // For now assume that the path will not + // be split. Else the intermediate goal + // will be reset in setPlan. } void DWBLocalPlanner::setPlan(const nav_2d_msgs::Path2D& path) { - pub_.publishGlobalPlan(path); - global_plan_ = path; + bool split_path; + planner_nh_.param("split_path", split_path, false); + + global_plan_segments_.clear(); + + if (split_path) + { + /* + Global planners like SBPL might create trajectories with complex + maneuvers, switching between driving forwards and backwards, where it is + crucial that the individual segments are carefully followed and completed + before starting the next. The "split_path" option allows to split the + given path in such segments and to treat each of them independently. + + The segmentation is computed as follows: + Given two poses in the path, we compute the vector v1 connecting them, + the vector of orientation given through the angle theta, check the dot + product of the vectors: If it is less than 0, the angle is over 90 deg, + which is a coarse approximation for "driving backwards", and for "driving + forwards" otherwise. In the special case that the vector connecting the + two poses is null we have to deal with in-place-rotations of the robot. + To avoid the robot from eagerly driving forward before having achieved the + correct orientation, these situations are grouped in a third category. + The path is then split into segments of the same movement-direction + (forward/backward/onlyRotation). When a cut is made, the last pose of the + last segment is copied, to be the first pose of the following segment, to + ensure a seamless path. + + The "global_plan_" variable is then set to the first segment. + In the "isGoalReached" method, which is repeatedly called, we check if + the intermediate goal - i.e., the end of the current segment - is reached, + and if so proceed to the next segment. + */ + ROS_INFO_NAMED("DWBLocalPlanner", "Splitting path..."); + + auto copy = path; + while (copy.poses.size() > 1) // need at least 2 poses in a path + { + // start a new segment + nav_2d_msgs::Path2D segment; + segment.header = path.header; + + // add the first pose + segment.poses.push_back(copy.poses[0]); + copy.poses.erase(copy.poses.begin()); + + // add the second pose and determine if we are going forward or backward + segment.poses.push_back(copy.poses[0]); + copy.poses.erase(copy.poses.begin()); + + // take the vector from the first to the second position and compare it + // to the orientation vector at the first position. If the angle is > 90 deg, + // assume we are driving backwards. + double v1[2] = + { + segment.poses[1].x - segment.poses[0].x, + segment.poses[1].y - segment.poses[0].y + }; + double v2[2] = + { + cos(segment.poses[0].theta), + sin(segment.poses[0].theta) + }; + double d = v1[0] * v2[0] + v1[1] * v2[1]; // dotproduct of the vectors. if < 0, the angle is over 90 degrees. + bool backwards = (d < 0); + bool onlyRotation = std::fabs(v1[0]) < 1e-5 && std::fabs(v1[1]) < 1e-5; + // if the translation vector is zero, the two positions are equal + // and the plan is to rotate on the spot. Since dwb would + // enthusiastically speed up at the start of a trajectory, + // even if it starts with in-place-rotation, it would + // miss the path by a lot. So let's split the path into + // forwards / backwards / onlyRotation. + + // add more poses while they lead into the same direction (driving forwards/backwards/onlyRotation) + while (!copy.poses.empty()) + { + // vector from the last pose in the segment to the next in the path + double v1[2] = + { + copy.poses[0].x - segment.poses.back().x, + copy.poses[0].y - segment.poses.back().y + }; + // orientation at the last pose in the segment + double v2[2] = + { + cos(segment.poses.back().theta), + sin(segment.poses.back().theta) + }; + double d = v1[0] * v2[0] + v1[1] * v2[1]; // dotproduct of the vectors. if < 0, the angle is over 90 degrees. + bool b = (d < 0); + bool rot = std::fabs(v1[0]) < 1e-5 && std::fabs(v1[1]) < 1e-5; + + if (b == backwards && rot == onlyRotation) + { + // same direction -> just add the pose + segment.poses.push_back(copy.poses[0]); + copy.poses.erase(copy.poses.begin()); + } + else + { + // direction changes -> add the last pose of the last segment back to + // the path, to start a new segment at the end of the last + copy.poses.insert(copy.poses.begin(), segment.poses.back()); + break; + } + } + + // add the created segment to the list + global_plan_segments_.push_back(segment); + } + if (global_plan_segments_.empty()) + { + // path doesn't have at least 2 poses, hence there is only one segment, which + // is the complete path. + global_plan_segments_.push_back(path); + } + + ROS_INFO_STREAM("Split path into " << global_plan_segments_.size() << " segments."); + } + else + { + // split_path option is disabled, hence there is only one segment, which + // is the complete path. + global_plan_segments_.push_back(path); + } + + // set the global_plan_ to be the first segment + global_plan_ = global_plan_segments_[0]; + global_plan_segments_.erase(global_plan_segments_.begin()); + + // publish not the complete path, but only the first segment. + pub_.publishGlobalPlan(global_plan_); + + // set the intermediate goal + intermediate_goal_pose_.header = global_plan_.header; + intermediate_goal_pose_.pose = global_plan_.poses.back(); + + resetPlugins(); + // prepare(...) to initialize the critics for the new segment does not need + // to be called here, as it is called at every computeVelocityCommands-call. +} + +void DWBLocalPlanner::resetPlugins() +{ traj_generator_->reset(); goal_checker_->reset(); for (TrajectoryCritic::Ptr critic : critics_) @@ -209,9 +380,10 @@ void DWBLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped& pose, const nav_ // Update time stamp of goal pose goal_pose_.header.stamp = pose.header.stamp; + intermediate_goal_pose_.header.stamp = pose.header.stamp; geometry_msgs::Pose2D local_start_pose = transformPoseToLocal(pose), - local_goal_pose = transformPoseToLocal(goal_pose_); + local_goal_pose = transformPoseToLocal(intermediate_goal_pose_); pub_.publishInputParams(costmap_->getInfo(), local_start_pose, velocity, local_goal_pose); From 48f56b7dbf539a582088d1f4af5ffb0919a85c1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 3 Jul 2020 15:24:41 +0200 Subject: [PATCH 4/6] Move splitPlan into path_ops --- dwb_local_planner/src/dwb_local_planner.cpp | 114 +------------------ nav_2d_utils/include/nav_2d_utils/path_ops.h | 29 +++++ nav_2d_utils/src/path_ops.cpp | 90 +++++++++++++++ 3 files changed, 122 insertions(+), 111 deletions(-) diff --git a/dwb_local_planner/src/dwb_local_planner.cpp b/dwb_local_planner/src/dwb_local_planner.cpp index 2ea5153..3ac4f26 100644 --- a/dwb_local_planner/src/dwb_local_planner.cpp +++ b/dwb_local_planner/src/dwb_local_planner.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -197,126 +198,17 @@ void DWBLocalPlanner::setPlan(const nav_2d_msgs::Path2D& path) bool split_path; planner_nh_.param("split_path", split_path, false); - global_plan_segments_.clear(); - if (split_path) { - /* - Global planners like SBPL might create trajectories with complex - maneuvers, switching between driving forwards and backwards, where it is - crucial that the individual segments are carefully followed and completed - before starting the next. The "split_path" option allows to split the - given path in such segments and to treat each of them independently. - - The segmentation is computed as follows: - Given two poses in the path, we compute the vector v1 connecting them, - the vector of orientation given through the angle theta, check the dot - product of the vectors: If it is less than 0, the angle is over 90 deg, - which is a coarse approximation for "driving backwards", and for "driving - forwards" otherwise. In the special case that the vector connecting the - two poses is null we have to deal with in-place-rotations of the robot. - To avoid the robot from eagerly driving forward before having achieved the - correct orientation, these situations are grouped in a third category. - The path is then split into segments of the same movement-direction - (forward/backward/onlyRotation). When a cut is made, the last pose of the - last segment is copied, to be the first pose of the following segment, to - ensure a seamless path. - - The "global_plan_" variable is then set to the first segment. - In the "isGoalReached" method, which is repeatedly called, we check if - the intermediate goal - i.e., the end of the current segment - is reached, - and if so proceed to the next segment. - */ ROS_INFO_NAMED("DWBLocalPlanner", "Splitting path..."); - - auto copy = path; - while (copy.poses.size() > 1) // need at least 2 poses in a path - { - // start a new segment - nav_2d_msgs::Path2D segment; - segment.header = path.header; - - // add the first pose - segment.poses.push_back(copy.poses[0]); - copy.poses.erase(copy.poses.begin()); - - // add the second pose and determine if we are going forward or backward - segment.poses.push_back(copy.poses[0]); - copy.poses.erase(copy.poses.begin()); - - // take the vector from the first to the second position and compare it - // to the orientation vector at the first position. If the angle is > 90 deg, - // assume we are driving backwards. - double v1[2] = - { - segment.poses[1].x - segment.poses[0].x, - segment.poses[1].y - segment.poses[0].y - }; - double v2[2] = - { - cos(segment.poses[0].theta), - sin(segment.poses[0].theta) - }; - double d = v1[0] * v2[0] + v1[1] * v2[1]; // dotproduct of the vectors. if < 0, the angle is over 90 degrees. - bool backwards = (d < 0); - bool onlyRotation = std::fabs(v1[0]) < 1e-5 && std::fabs(v1[1]) < 1e-5; - // if the translation vector is zero, the two positions are equal - // and the plan is to rotate on the spot. Since dwb would - // enthusiastically speed up at the start of a trajectory, - // even if it starts with in-place-rotation, it would - // miss the path by a lot. So let's split the path into - // forwards / backwards / onlyRotation. - - // add more poses while they lead into the same direction (driving forwards/backwards/onlyRotation) - while (!copy.poses.empty()) - { - // vector from the last pose in the segment to the next in the path - double v1[2] = - { - copy.poses[0].x - segment.poses.back().x, - copy.poses[0].y - segment.poses.back().y - }; - // orientation at the last pose in the segment - double v2[2] = - { - cos(segment.poses.back().theta), - sin(segment.poses.back().theta) - }; - double d = v1[0] * v2[0] + v1[1] * v2[1]; // dotproduct of the vectors. if < 0, the angle is over 90 degrees. - bool b = (d < 0); - bool rot = std::fabs(v1[0]) < 1e-5 && std::fabs(v1[1]) < 1e-5; - - if (b == backwards && rot == onlyRotation) - { - // same direction -> just add the pose - segment.poses.push_back(copy.poses[0]); - copy.poses.erase(copy.poses.begin()); - } - else - { - // direction changes -> add the last pose of the last segment back to - // the path, to start a new segment at the end of the last - copy.poses.insert(copy.poses.begin(), segment.poses.back()); - break; - } - } - - // add the created segment to the list - global_plan_segments_.push_back(segment); - } - if (global_plan_segments_.empty()) - { - // path doesn't have at least 2 poses, hence there is only one segment, which - // is the complete path. - global_plan_segments_.push_back(path); - } - + global_plan_segments_ = nav_2d_utils::splitPlan(path); ROS_INFO_STREAM("Split path into " << global_plan_segments_.size() << " segments."); } else { // split_path option is disabled, hence there is only one segment, which // is the complete path. + global_plan_segments_.clear(); global_plan_segments_.push_back(path); } diff --git a/nav_2d_utils/include/nav_2d_utils/path_ops.h b/nav_2d_utils/include/nav_2d_utils/path_ops.h index 187b762..a124131 100644 --- a/nav_2d_utils/include/nav_2d_utils/path_ops.h +++ b/nav_2d_utils/include/nav_2d_utils/path_ops.h @@ -36,6 +36,7 @@ #define NAV_2D_UTILS_PATH_OPS_H #include +#include namespace nav_2d_utils { @@ -83,6 +84,34 @@ nav_2d_msgs::Path2D compressPlan(const nav_2d_msgs::Path2D& input_path, double e */ void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta = 0.0); +/** + * @brief Split the plan into segments when it is switching between going forwards / backwards / pure rotation + * + * Global planners like SBPL might create trajectories with complex + * maneuvers, switching between driving forwards and backwards, where it is + * crucial that the individual segments are carefully followed and completed + * before starting the next. This function splits the given path into such + * segments, so that they can be independently treated as separate plans. + + * The segmentation is computed as follows: + * Given two poses in the path, we compute the vector v1 connecting them, + * the vector of orientation given through the angle theta, check the dot + * product of the vectors: If it is less than 0, the angle is over 90 deg, + * which is a coarse approximation for "driving backwards", and for "driving + * forwards" otherwise. In the special case that the vector connecting the + * two poses is null we have to deal with in-place-rotations of the robot. + * To avoid the robot from eagerly driving forward before having achieved the + * correct orientation, these situations are grouped in a third category, + * "pure rotation". The path is then split into segments of the same movement + * direction (forward/backward/pureRotation). When a cut is made, the last pose of the + * last segment is copied to be the first pose of the following segment in order to + * ensure a seamless path. + * + * @param global_plan_in input plan + * @return vector of plan segments + */ +std::vector splitPlan(const nav_2d_msgs::Path2D &global_plan_in); + } // namespace nav_2d_utils #endif // NAV_2D_UTILS_PATH_OPS_H diff --git a/nav_2d_utils/src/path_ops.cpp b/nav_2d_utils/src/path_ops.cpp index a91283a..6366b23 100644 --- a/nav_2d_utils/src/path_ops.cpp +++ b/nav_2d_utils/src/path_ops.cpp @@ -189,4 +189,94 @@ void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta) pose.theta = theta; path.poses.push_back(pose); } + +std::vector splitPlan(const nav_2d_msgs::Path2D &global_plan_in) +{ + auto copy = global_plan_in; + std::vector global_plan_segments; // Path segments of same movement direction + // (forward/backward/in-place rotation) + + while (copy.poses.size() > 1) // need at least 2 poses in a path + { + // start a new segment + nav_2d_msgs::Path2D segment; + segment.header = global_plan_in.header; + + // add the first pose + segment.poses.push_back(copy.poses[0]); + copy.poses.erase(copy.poses.begin()); + + // add the second pose and determine if we are going forward or backward + segment.poses.push_back(copy.poses[0]); + copy.poses.erase(copy.poses.begin()); + + // take the vector from the first to the second position and compare it + // to the orientation vector at the first position. If the angle is > 90 deg, + // assume we are driving backwards. + double v1[2] = + { + segment.poses[1].x - segment.poses[0].x, + segment.poses[1].y - segment.poses[0].y + }; + double v2[2] = + { + cos(segment.poses[0].theta), + sin(segment.poses[0].theta) + }; + double d = v1[0] * v2[0] + v1[1] * v2[1]; // dot product of the vectors. if < 0, the angle is over 90 degrees. + bool backwards = (d < 0); + bool pureRotation = fabs(v1[0]) < 1e-5 && fabs(v1[1]) < 1e-5; + // if the translation vector is zero, the two positions are equal + // and the plan is to rotate on the spot. Since dwb would + // enthusiastically speed up at the start of a trajectory, + // even if it starts with in-place-rotation, it would + // miss the path by a lot. So let's split the path into + // forwards / backwards / pureRotation. + + // add more poses while they lead into the same direction (driving forwards/backwards/pureRotation) + while (!copy.poses.empty()) + { + // vector from the last pose in the segment to the next in the path + double v1[2] = + { + copy.poses[0].x - segment.poses.back().x, + copy.poses[0].y - segment.poses.back().y + }; + // orientation at the last pose in the segment + double v2[2] = + { + cos(segment.poses.back().theta), + sin(segment.poses.back().theta) + }; + double d = v1[0] * v2[0] + v1[1] * v2[1]; // dot product of the vectors. if < 0, the angle is over 90 degrees. + bool b = (d < 0); + bool rot = fabs(v1[0]) < 1e-5 && fabs(v1[1]) < 1e-5; + + if (b == backwards && rot == pureRotation) + { + // same direction -> just add the pose + segment.poses.push_back(copy.poses[0]); + copy.poses.erase(copy.poses.begin()); + } + else + { + // direction changes -> add the last pose of the last segment back to + // the path, to start a new segment at the end of the last + copy.poses.insert(copy.poses.begin(), segment.poses.back()); + break; + } + } + + // add the created segment to the list + global_plan_segments.push_back(segment); + } + if (global_plan_segments.empty()) + { + // global_plan_in doesn't have at least 2 poses, hence there is only one segment, which + // is the complete path. + global_plan_segments.push_back(global_plan_in); + } + + return global_plan_segments; +} } // namespace nav_2d_utils From 2f192a855d3ea9bfff13ab93db971d566133a7cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 3 Jul 2020 15:40:00 +0200 Subject: [PATCH 5/6] Extract splitPlan parameter epsilon --- nav_2d_utils/include/nav_2d_utils/path_ops.h | 3 ++- nav_2d_utils/src/path_ops.cpp | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/nav_2d_utils/include/nav_2d_utils/path_ops.h b/nav_2d_utils/include/nav_2d_utils/path_ops.h index a124131..a675fda 100644 --- a/nav_2d_utils/include/nav_2d_utils/path_ops.h +++ b/nav_2d_utils/include/nav_2d_utils/path_ops.h @@ -108,9 +108,10 @@ void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta = 0.0); * ensure a seamless path. * * @param global_plan_in input plan + * @param epsilon maximum linear translation threshold for "pure rotation" segments * @return vector of plan segments */ -std::vector splitPlan(const nav_2d_msgs::Path2D &global_plan_in); +std::vector splitPlan(const nav_2d_msgs::Path2D &global_plan_in, double epsilon = 1e-5); } // namespace nav_2d_utils diff --git a/nav_2d_utils/src/path_ops.cpp b/nav_2d_utils/src/path_ops.cpp index 6366b23..692fab5 100644 --- a/nav_2d_utils/src/path_ops.cpp +++ b/nav_2d_utils/src/path_ops.cpp @@ -190,7 +190,7 @@ void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta) path.poses.push_back(pose); } -std::vector splitPlan(const nav_2d_msgs::Path2D &global_plan_in) +std::vector splitPlan(const nav_2d_msgs::Path2D &global_plan_in, double epsilon) { auto copy = global_plan_in; std::vector global_plan_segments; // Path segments of same movement direction @@ -225,7 +225,7 @@ std::vector splitPlan(const nav_2d_msgs::Path2D &global_pla }; double d = v1[0] * v2[0] + v1[1] * v2[1]; // dot product of the vectors. if < 0, the angle is over 90 degrees. bool backwards = (d < 0); - bool pureRotation = fabs(v1[0]) < 1e-5 && fabs(v1[1]) < 1e-5; + bool pureRotation = fabs(v1[0]) < epsilon && fabs(v1[1]) < epsilon; // if the translation vector is zero, the two positions are equal // and the plan is to rotate on the spot. Since dwb would // enthusiastically speed up at the start of a trajectory, @@ -250,7 +250,7 @@ std::vector splitPlan(const nav_2d_msgs::Path2D &global_pla }; double d = v1[0] * v2[0] + v1[1] * v2[1]; // dot product of the vectors. if < 0, the angle is over 90 degrees. bool b = (d < 0); - bool rot = fabs(v1[0]) < 1e-5 && fabs(v1[1]) < 1e-5; + bool rot = fabs(v1[0]) < epsilon && fabs(v1[1]) < epsilon; if (b == backwards && rot == pureRotation) { From 505db289638a56405f139d17f58b78284d43e375 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Fri, 3 Jul 2020 16:21:02 +0200 Subject: [PATCH 6/6] Add test for splitPlan --- nav_2d_utils/CMakeLists.txt | 3 + nav_2d_utils/test/split_plan_test.cpp | 424 ++++++++++++++++++++++++++ 2 files changed, 427 insertions(+) create mode 100644 nav_2d_utils/test/split_plan_test.cpp diff --git a/nav_2d_utils/CMakeLists.txt b/nav_2d_utils/CMakeLists.txt index b8db3de..3f6045f 100644 --- a/nav_2d_utils/CMakeLists.txt +++ b/nav_2d_utils/CMakeLists.txt @@ -65,6 +65,9 @@ if(CATKIN_ENABLE_TESTING) add_rostest_gtest(param_tests test/param_tests.launch test/param_tests.cpp) target_link_libraries(param_tests polygons ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) + + catkin_add_gtest(split_plan_test test/split_plan_test.cpp) + target_link_libraries(split_plan_test path_ops ${catkin_LIBRARIES}) endif() install(TARGETS conversions path_ops polygons diff --git a/nav_2d_utils/test/split_plan_test.cpp b/nav_2d_utils/test/split_plan_test.cpp new file mode 100644 index 0000000..5022a1d --- /dev/null +++ b/nav_2d_utils/test/split_plan_test.cpp @@ -0,0 +1,424 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include + +using nav_2d_utils::splitPlan; +using nav_2d_utils::addPose; + +TEST(SplitPlanTest, split_plan_test) +{ + nav_2d_msgs::Path2D path; + // ======= segment 0 ========= + addPose(path, 8.975, 9.225, 0.7854); + addPose(path, 8.9694, 9.2194, 0.7854); + addPose(path, 8.9639, 9.2139, 0.7854); + addPose(path, 8.9583, 9.2083, 0.7854); + addPose(path, 8.9528, 9.2028, 0.7854); + addPose(path, 8.9472, 9.1972, 0.7854); + addPose(path, 8.9417, 9.1917, 0.7854); + addPose(path, 8.9361, 9.1861, 0.7854); + addPose(path, 8.9306, 9.1806, 0.7854); + addPose(path, 8.925, 9.175, 0.7854); + addPose(path, 8.9194, 9.1694, 0.7854); + addPose(path, 8.9139, 9.1639, 0.7854); + addPose(path, 8.9083, 9.1583, 0.7854); + addPose(path, 8.9028, 9.1528, 0.7854); + addPose(path, 8.8972, 9.1472, 0.7854); + addPose(path, 8.8917, 9.1417, 0.7854); + addPose(path, 8.8861, 9.1361, 0.7854); + addPose(path, 8.8806, 9.1306, 0.7854); + addPose(path, 8.875, 9.125, 0.7854); + addPose(path, 8.8694, 9.1194, 0.7854); + addPose(path, 8.8639, 9.1139, 0.7854); + addPose(path, 8.8583, 9.1083, 0.7854); + addPose(path, 8.8528, 9.1028, 0.7854); + addPose(path, 8.8472, 9.0972, 0.7854); + addPose(path, 8.8417, 9.0917, 0.7854); + addPose(path, 8.8361, 9.0861, 0.7854); + addPose(path, 8.8306, 9.0806, 0.7854); + addPose(path, 8.825, 9.075, 0.7854); + addPose(path, 8.8194, 9.0694, 0.7854); + addPose(path, 8.8139, 9.0639, 0.7854); + addPose(path, 8.8083, 9.0583, 0.7854); + addPose(path, 8.8028, 9.0528, 0.7854); + addPose(path, 8.7972, 9.0472, 0.7854); + addPose(path, 8.7917, 9.0417, 0.7854); + addPose(path, 8.7861, 9.0361, 0.7854); + addPose(path, 8.7806, 9.0306, 0.7854); + addPose(path, 8.775, 9.025, 0.7854); + addPose(path, 8.7694, 9.0194, 0.7854); + addPose(path, 8.7639, 9.0139, 0.7854); + addPose(path, 8.7583, 9.0083, 0.7854); + addPose(path, 8.7528, 9.0028, 0.7854); + addPose(path, 8.7472, 8.9972, 0.7854); + addPose(path, 8.7417, 8.9917, 0.7854); + addPose(path, 8.7361, 8.9861, 0.7854); + addPose(path, 8.7306, 8.9806, 0.7854); + addPose(path, 8.725, 8.975, 0.7854); + addPose(path, 8.7194, 8.9694, 0.7854); + addPose(path, 8.7139, 8.9639, 0.7854); + addPose(path, 8.7083, 8.9583, 0.7854); + addPose(path, 8.7028, 8.9528, 0.7854); + addPose(path, 8.6972, 8.9472, 0.7854); + addPose(path, 8.6917, 8.9417, 0.7854); + addPose(path, 8.6861, 8.9361, 0.7854); + addPose(path, 8.6806, 8.9306, 0.7854); + addPose(path, 8.675, 8.925, 0.7854); + addPose(path, 8.6694, 8.9194, 0.7854); + addPose(path, 8.6639, 8.9139, 0.7854); + addPose(path, 8.6583, 8.9083, 0.7854); + addPose(path, 8.6528, 8.9028, 0.7854); + addPose(path, 8.6472, 8.8972, 0.7854); + addPose(path, 8.6417, 8.8917, 0.7854); + addPose(path, 8.6361, 8.8861, 0.7854); + addPose(path, 8.6306, 8.8806, 0.7854); + addPose(path, 8.625, 8.875, 0.7854); + addPose(path, 8.6194, 8.8694, 0.7854); + addPose(path, 8.6139, 8.8639, 0.7854); + addPose(path, 8.6083, 8.8583, 0.7854); + addPose(path, 8.6028, 8.8528, 0.7854); + addPose(path, 8.5972, 8.8472, 0.7854); + addPose(path, 8.5917, 8.8417, 0.7854); + addPose(path, 8.5861, 8.8361, 0.7854); + addPose(path, 8.5806, 8.8306, 0.7854); + addPose(path, 8.575, 8.825, 0.7854); + addPose(path, 8.5694, 8.8194, 0.7854); + addPose(path, 8.5639, 8.8139, 0.7854); + addPose(path, 8.5583, 8.8083, 0.7854); + addPose(path, 8.5528, 8.8028, 0.7854); + addPose(path, 8.5472, 8.7972, 0.7854); + addPose(path, 8.5417, 8.7917, 0.7854); + addPose(path, 8.5361, 8.7861, 0.7854); + addPose(path, 8.5306, 8.7806, 0.7854); + addPose(path, 8.525, 8.775, 0.7854); + addPose(path, 8.5194, 8.7694, 0.7854); + addPose(path, 8.5139, 8.7639, 0.7854); + addPose(path, 8.5083, 8.7583, 0.7854); + addPose(path, 8.5028, 8.7528, 0.7854); + addPose(path, 8.4972, 8.7472, 0.7854); + addPose(path, 8.4917, 8.7417, 0.7854); + addPose(path, 8.4861, 8.7361, 0.7854); + addPose(path, 8.4806, 8.7306, 0.7854); + addPose(path, 8.475, 8.725, 0.7854); + addPose(path, 8.4694, 8.7194, 0.7854); + addPose(path, 8.4639, 8.7139, 0.7854); + addPose(path, 8.4583, 8.7083, 0.7854); + addPose(path, 8.4528, 8.7028, 0.7854); + addPose(path, 8.4472, 8.6972, 0.7854); + addPose(path, 8.4417, 8.6917, 0.7854); + addPose(path, 8.4361, 8.6861, 0.7854); + addPose(path, 8.4306, 8.6806, 0.7854); + addPose(path, 8.425, 8.675, 0.7854); + addPose(path, 8.4194, 8.6694, 0.7854); + addPose(path, 8.4139, 8.6639, 0.7854); + addPose(path, 8.4083, 8.6583, 0.7854); + addPose(path, 8.4028, 8.6528, 0.7854); + addPose(path, 8.3972, 8.6472, 0.7854); + addPose(path, 8.3917, 8.6417, 0.7854); + addPose(path, 8.3861, 8.6361, 0.7854); + addPose(path, 8.3806, 8.6306, 0.7854); + addPose(path, 8.375, 8.625, 0.7854); + addPose(path, 8.3694, 8.6194, 0.7854); + addPose(path, 8.3639, 8.6139, 0.7854); + addPose(path, 8.3583, 8.6083, 0.7854); + addPose(path, 8.3528, 8.6028, 0.7854); + addPose(path, 8.3472, 8.5972, 0.7854); + addPose(path, 8.3417, 8.5917, 0.7854); + addPose(path, 8.3361, 8.5861, 0.7854); + addPose(path, 8.3306, 8.5806, 0.7854); + addPose(path, 8.325, 8.575, 0.7854); + addPose(path, 8.3194, 8.5694, 0.7854); + addPose(path, 8.3139, 8.5639, 0.7854); + addPose(path, 8.3083, 8.5583, 0.7854); + addPose(path, 8.3028, 8.5528, 0.7854); + addPose(path, 8.2972, 8.5472, 0.7854); + addPose(path, 8.2917, 8.5417, 0.7854); + addPose(path, 8.2861, 8.5361, 0.7854); + addPose(path, 8.2806, 8.5306, 0.7854); + addPose(path, 8.275, 8.525, 0.7854); + addPose(path, 8.2694, 8.5194, 0.7854); + addPose(path, 8.2639, 8.5139, 0.7854); + addPose(path, 8.2583, 8.5083, 0.7854); + addPose(path, 8.2528, 8.5028, 0.7854); + addPose(path, 8.2472, 8.4972, 0.7854); + addPose(path, 8.2417, 8.4917, 0.7854); + addPose(path, 8.2361, 8.4861, 0.7854); + addPose(path, 8.2306, 8.4806, 0.7854); + addPose(path, 8.225, 8.475, 0.7854); + addPose(path, 8.2194, 8.4694, 0.7854); + addPose(path, 8.2139, 8.4639, 0.7854); + addPose(path, 8.2083, 8.4583, 0.7854); + addPose(path, 8.2028, 8.4528, 0.7854); + addPose(path, 8.1972, 8.4472, 0.7854); + addPose(path, 8.1917, 8.4417, 0.7854); + addPose(path, 8.1861, 8.4361, 0.7854); + addPose(path, 8.1806, 8.4306, 0.7854); + addPose(path, 8.175, 8.425, 0.7854); + addPose(path, 8.1694, 8.4194, 0.7854); + addPose(path, 8.1639, 8.4139, 0.7854); + addPose(path, 8.1583, 8.4083, 0.7854); + addPose(path, 8.1528, 8.4028, 0.7854); + addPose(path, 8.1472, 8.3972, 0.7854); + addPose(path, 8.1417, 8.3917, 0.7854); + addPose(path, 8.1361, 8.3861, 0.7854); + addPose(path, 8.1306, 8.3806, 0.7854); + addPose(path, 8.125, 8.375, 0.7854); + addPose(path, 8.1194, 8.3694, 0.7854); + addPose(path, 8.1139, 8.3639, 0.7854); + addPose(path, 8.1083, 8.3583, 0.7854); + addPose(path, 8.1028, 8.3528, 0.7854); + addPose(path, 8.0972, 8.3472, 0.7854); + addPose(path, 8.0917, 8.3417, 0.7854); + addPose(path, 8.0861, 8.3361, 0.7854); + addPose(path, 8.0806, 8.3306, 0.7854); + addPose(path, 8.075, 8.325, 0.7854); + addPose(path, 8.0694, 8.3194, 0.7854); + addPose(path, 8.0639, 8.3139, 0.7854); + addPose(path, 8.0583, 8.3083, 0.7854); + addPose(path, 8.0528, 8.3028, 0.7854); + addPose(path, 8.0472, 8.2972, 0.7854); + addPose(path, 8.0417, 8.2917, 0.7854); + addPose(path, 8.0361, 8.2861, 0.7854); + addPose(path, 8.0306, 8.2806, 0.7854); + addPose(path, 8.025, 8.275, 0.7854); + addPose(path, 8.0194, 8.2694, 0.7854); + addPose(path, 8.0139, 8.2639, 0.7854); + addPose(path, 8.0083, 8.2583, 0.7854); + addPose(path, 8.0028, 8.2528, 0.7854); + addPose(path, 7.9972, 8.2472, 0.7854); + addPose(path, 7.9917, 8.2417, 0.7854); + addPose(path, 7.9861, 8.2361, 0.7854); + addPose(path, 7.9806, 8.2306, 0.7854); + addPose(path, 7.975, 8.225, 0.7854); + addPose(path, 7.9694, 8.2194, 0.7854); + addPose(path, 7.9639, 8.2139, 0.7854); + addPose(path, 7.9583, 8.2083, 0.7854); + addPose(path, 7.9528, 8.2028, 0.7854); + addPose(path, 7.9472, 8.1972, 0.7854); + addPose(path, 7.9417, 8.1917, 0.7854); + addPose(path, 7.9361, 8.1861, 0.7854); + addPose(path, 7.9306, 8.1806, 0.7854); + addPose(path, 7.925, 8.175, 0.7854); + addPose(path, 7.9194, 8.1694, 0.7854); + addPose(path, 7.9139, 8.1639, 0.7854); + addPose(path, 7.9083, 8.1583, 0.7854); + addPose(path, 7.9028, 8.1528, 0.7854); + addPose(path, 7.8972, 8.1472, 0.7854); + addPose(path, 7.8917, 8.1417, 0.7854); + addPose(path, 7.8861, 8.1361, 0.7854); + addPose(path, 7.8806, 8.1306, 0.7854); + addPose(path, 7.875, 8.125, 0.7854); + addPose(path, 7.8694, 8.1194, 0.7854); + addPose(path, 7.8639, 8.1139, 0.7854); + addPose(path, 7.8583, 8.1083, 0.7854); + addPose(path, 7.8528, 8.1028, 0.7854); + addPose(path, 7.8472, 8.0972, 0.7854); + addPose(path, 7.8417, 8.0917, 0.7854); + addPose(path, 7.8361, 8.0861, 0.7854); + addPose(path, 7.8306, 8.0806, 0.7854); + addPose(path, 7.825, 8.075, 0.7854); + addPose(path, 7.8194, 8.0694, 0.7854); + addPose(path, 7.8139, 8.0639, 0.7854); + addPose(path, 7.8083, 8.0583, 0.7854); + addPose(path, 7.8028, 8.0528, 0.7854); + addPose(path, 7.7972, 8.0472, 0.7854); + addPose(path, 7.7917, 8.0417, 0.7854); + addPose(path, 7.7861, 8.0361, 0.7854); + addPose(path, 7.7806, 8.0306, 0.7854); + addPose(path, 7.775, 8.025, 0.7854); + addPose(path, 7.7694, 8.0194, 0.7854); + addPose(path, 7.7639, 8.0139, 0.7854); + addPose(path, 7.7583, 8.0083, 0.7854); + addPose(path, 7.7528, 8.0028, 0.7854); + addPose(path, 7.7472, 7.9972, 0.7854); + addPose(path, 7.7417, 7.9917, 0.7854); + addPose(path, 7.7361, 7.9861, 0.7854); + addPose(path, 7.7306, 7.9806, 0.7854); + addPose(path, 7.725, 7.975, 0.7854); + addPose(path, 7.7194, 7.9694, 0.7854); + addPose(path, 7.7139, 7.9639, 0.7854); + addPose(path, 7.7083, 7.9583, 0.7854); + addPose(path, 7.7028, 7.9528, 0.7854); + addPose(path, 7.6972, 7.9472, 0.7854); + addPose(path, 7.6917, 7.9417, 0.7854); + addPose(path, 7.6861, 7.9361, 0.7854); + addPose(path, 7.6806, 7.9306, 0.7854); + addPose(path, 7.675, 7.925, 0.7854); + addPose(path, 7.6694, 7.9194, 0.7854); + addPose(path, 7.6639, 7.9139, 0.7854); + addPose(path, 7.6583, 7.9083, 0.7854); + addPose(path, 7.6528, 7.9028, 0.7854); + addPose(path, 7.6472, 7.8972, 0.7854); + addPose(path, 7.6417, 7.8917, 0.7854); + addPose(path, 7.6361, 7.8861, 0.7854); + addPose(path, 7.6306, 7.8806, 0.7854); + addPose(path, 7.625, 7.875, 0.7854); + addPose(path, 7.6194, 7.8694, 0.7854); + addPose(path, 7.6139, 7.8639, 0.7854); + addPose(path, 7.6083, 7.8583, 0.7854); + addPose(path, 7.6028, 7.8528, 0.7854); + addPose(path, 7.5972, 7.8472, 0.7854); + addPose(path, 7.5917, 7.8417, 0.7854); + addPose(path, 7.5861, 7.8361, 0.7854); + addPose(path, 7.5806, 7.8306, 0.7854); + addPose(path, 7.575, 7.825, 0.7854); + addPose(path, 7.5694, 7.8194, 0.7854); + addPose(path, 7.5639, 7.8139, 0.7854); + addPose(path, 7.5583, 7.8083, 0.7854); + addPose(path, 7.5528, 7.8028, 0.7854); + addPose(path, 7.5472, 7.7972, 0.7854); + addPose(path, 7.5417, 7.7917, 0.7854); + addPose(path, 7.5361, 7.7861, 0.7854); + addPose(path, 7.5306, 7.7806, 0.7854); + addPose(path, 7.525, 7.775, 0.7854); + + // ======= segment 1 ========= + addPose(path, 7.5591, 7.8091, 0.7854); + addPose(path, 7.5934, 7.8428, 0.7557); + addPose(path, 7.6293, 7.875, 0.7039); + addPose(path, 7.6668, 7.9052, 0.652); + addPose(path, 7.7059, 7.9334, 0.6001); + addPose(path, 7.7463, 7.9596, 0.5483); + addPose(path, 7.7881, 7.9836, 0.4964); + addPose(path, 7.831, 8.0054, 0.4446); + addPose(path, 7.875, 8.025, 0.3927); + + // ======= segment 2 ========= + addPose(path, 7.8639, 8.0194, 0.3927); + addPose(path, 7.8528, 8.0139, 0.3927); + addPose(path, 7.8417, 8.0083, 0.3927); + addPose(path, 7.8306, 8.0028, 0.3927); + addPose(path, 7.8194, 7.9972, 0.3927); + addPose(path, 7.8083, 7.9917, 0.3927); + addPose(path, 7.7972, 7.9861, 0.3927); + addPose(path, 7.7861, 7.9806, 0.3927); + addPose(path, 7.775, 7.975, 0.3927); + + // ======= segment 3 ========= + addPose(path, 7.8127, 7.9906, 0.3927); + addPose(path, 7.8504, 8.0062, 0.3927); + addPose(path, 7.8881, 8.0218, 0.3927); + addPose(path, 7.9258, 8.0373, 0.3736); + addPose(path, 7.9643, 8.0508, 0.2989); + addPose(path, 8.0037, 8.0613, 0.2242); + addPose(path, 8.0437, 8.0689, 0.1494); + addPose(path, 8.0842, 8.0735, 0.0747); + addPose(path, 8.125, 8.075, 0); + addPose(path, 8.1702, 8.075, 0); + addPose(path, 8.2154, 8.075, 0); + addPose(path, 8.2605, 8.075, 0); + addPose(path, 8.3057, 8.0742, -0.0488); + addPose(path, 8.3507, 8.0705, -0.1176); + addPose(path, 8.3953, 8.0636, -0.1864); + addPose(path, 8.4394, 8.0537, -0.2551); + addPose(path, 8.4827, 8.0408, -0.3239); + addPose(path, 8.525, 8.025, -0.392685); + addPose(path, 8.5361, 8.0194, -0.392685); + addPose(path, 8.5472, 8.0139, -0.392685); + addPose(path, 8.5583, 8.0083, -0.392685); + addPose(path, 8.5694, 8.0028, -0.392685); + addPose(path, 8.5806, 7.9972, -0.392685); + addPose(path, 8.5917, 7.9917, -0.392685); + addPose(path, 8.6028, 7.9861, -0.392685); + addPose(path, 8.6139, 7.9806, -0.392685); + addPose(path, 8.625, 7.975, -0.392685); + addPose(path, 8.6361, 7.9694, -0.392685); + addPose(path, 8.6472, 7.9639, -0.392685); + addPose(path, 8.6583, 7.9583, -0.392685); + addPose(path, 8.6694, 7.9528, -0.392685); + addPose(path, 8.6806, 7.9472, -0.392685); + addPose(path, 8.6917, 7.9417, -0.392685); + addPose(path, 8.7028, 7.9361, -0.392685); + addPose(path, 8.7139, 7.9306, -0.392685); + addPose(path, 8.725, 7.925, -0.392685); + addPose(path, 8.7361, 7.9194, -0.392685); + addPose(path, 8.7472, 7.9139, -0.392685); + addPose(path, 8.7583, 7.9083, -0.392685); + addPose(path, 8.7694, 7.9028, -0.392685); + addPose(path, 8.7806, 7.8972, -0.392685); + addPose(path, 8.7917, 7.8917, -0.392685); + addPose(path, 8.8028, 7.8861, -0.392685); + addPose(path, 8.8139, 7.8806, -0.392685); + addPose(path, 8.825, 7.875, -0.392685); + addPose(path, 8.8361, 7.8694, -0.392685); + addPose(path, 8.8472, 7.8639, -0.392685); + addPose(path, 8.8583, 7.8583, -0.392685); + addPose(path, 8.8694, 7.8528, -0.392685); + addPose(path, 8.8806, 7.8472, -0.392685); + addPose(path, 8.8917, 7.8417, -0.392685); + addPose(path, 8.9028, 7.8361, -0.392685); + addPose(path, 8.9139, 7.8306, -0.392685); + addPose(path, 8.925, 7.825, -0.392685); + addPose(path, 8.9361, 7.8194, -0.392685); + addPose(path, 8.9472, 7.8139, -0.392685); + addPose(path, 8.9583, 7.8083, -0.392685); + addPose(path, 8.9694, 7.8028, -0.392685); + addPose(path, 8.9806, 7.7972, -0.392685); + addPose(path, 8.9917, 7.7917, -0.392685); + addPose(path, 9.0028, 7.7861, -0.392685); + addPose(path, 9.0139, 7.7806, -0.392685); + addPose(path, 9.025, 7.775, -0.392685); + addPose(path, 9.0361, 7.7694, -0.392685); + addPose(path, 9.0472, 7.7639, -0.392685); + addPose(path, 9.0583, 7.7583, -0.392685); + addPose(path, 9.0694, 7.7528, -0.392685); + addPose(path, 9.0806, 7.7472, -0.392685); + addPose(path, 9.0917, 7.7417, -0.392685); + addPose(path, 9.1028, 7.7361, -0.392685); + addPose(path, 9.1139, 7.7306, -0.392685); + addPose(path, 9.125, 7.725, -0.392685); + addPose(path, 9.1627, 7.7094, -0.392685); + addPose(path, 9.2004, 7.6938, -0.392685); + addPose(path, 9.2381, 7.6782, -0.392685); + addPose(path, 9.2758, 7.6627, -0.373585); + addPose(path, 9.3143, 7.6492, -0.298885); + addPose(path, 9.3537, 7.6387, -0.224185); + addPose(path, 9.3937, 7.6311, -0.149485); + addPose(path, 9.4342, 7.6265, -0.0746853); + + std::vector segments = splitPlan(path); + EXPECT_EQ(4U, segments.size()); + EXPECT_EQ(262U, segments[0].poses.size()); + EXPECT_EQ(10U, segments[1].poses.size()); + EXPECT_EQ(10U, segments[2].poses.size()); + EXPECT_EQ(81U, segments[3].poses.size()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}