From 5ffeb9b6cba4698ece11d1cf2d4307e941bb9c9e Mon Sep 17 00:00:00 2001 From: Kevin Jaget Date: Mon, 12 Sep 2022 13:07:21 -0400 Subject: [PATCH 1/3] Add look-at mode for generating paths Add code to allow the robot to look at arbitrary positions during path following. This supplements the current behavior where the robot's orientation between waypoints is a spline connecting the orientation at each waypoint. Instead, the robot can look at a fixed position while moving, at the next waypoint, or rotating at a constant velocity --- .../base_trajectory/src/base_trajectory.cpp | 57 ++++++++++++++++--- .../src/base_trajectory_msgs/CMakeLists.txt | 2 - .../src/base_trajectory_msgs/package.xml | 1 - .../srv/GenerateSpline.srv | 8 +++ 4 files changed, 56 insertions(+), 12 deletions(-) diff --git a/zebROS_ws/src/base_trajectory/src/base_trajectory.cpp b/zebROS_ws/src/base_trajectory/src/base_trajectory.cpp index f38c23e8b..ddc3be538 100644 --- a/zebROS_ws/src/base_trajectory/src/base_trajectory.cpp +++ b/zebROS_ws/src/base_trajectory/src/base_trajectory.cpp @@ -942,7 +942,9 @@ bool evaluateTrajectory(T &cost, const XYTTrajectory &trajectory, const geometry_msgs::TransformStamped &pathToMapTransform, SampleTrajectoryImplCpu &sampleTrajectory, - const OptParamsList &optParams) + const OptParamsList &optParams, + const std::vector &rotateMode, + const std::vector &rotateData) { // arcLengthTrajectory takes in a time and returns the x-y-theta distance // traveled up to that time. @@ -1194,7 +1196,9 @@ void trajectoryToSplineResponseMsg(base_trajectory_msgs::GenerateSpline::Respons const std::vector &jointNames, const geometry_msgs::TransformStamped &pathToMapTransform, SampleTrajectoryImplCpu &sampleTrajectory, - const OptParamsList &optParams) + const OptParamsList &optParams, + const std::vector &rotateMode, + const std::vector &rotateData) { out_msg.orient_coefs.resize(trajectory[0].size()); out_msg.x_coefs.resize(trajectory[0].size()); @@ -1247,7 +1251,7 @@ void trajectoryToSplineResponseMsg(base_trajectory_msgs::GenerateSpline::Respons std::vector equalArcLengthTimes; std::vector remappedTimes; std::vector vTrans; - if (!evaluateTrajectory(cost, segCosts, equalArcLengthTimes, remappedTimes, vTrans, trajectory, pathToMapTransform, sampleTrajectory, optParams)) + if (!evaluateTrajectory(cost, segCosts, equalArcLengthTimes, remappedTimes, vTrans, trajectory, pathToMapTransform, sampleTrajectory, optParams, rotateMode, rotateData)) { ROS_ERROR_STREAM("base_trajectory_node trajectoryToSplineResponseMsg : evaluateTrajectory() returned false"); return; @@ -1385,6 +1389,8 @@ struct NLOPTParams OptParamsList &optParams, const std::vector &jointNames, const geometry_msgs::TransformStamped &pathToMapTransform, + const std::vector &rotateMode, + const std::vector &rotateData, SampleTrajectoryImplCpu &sampleTrajectory, SegCosts &segCosts, size_t &optimizationAttempts) @@ -1393,6 +1399,8 @@ struct NLOPTParams , optParams_(optParams) , jointNames_(jointNames) , pathToMapTransform_(pathToMapTransform) + , rotateMode_(rotateMode) + , rotateData_(rotateData) , sampleTrajectory_(sampleTrajectory) , segCosts_(segCosts) , optimizationAttempts_(optimizationAttempts) @@ -1406,6 +1414,8 @@ struct NLOPTParams OptParamsList &optParams_; const std::vector &jointNames_; const geometry_msgs::TransformStamped &pathToMapTransform_; + const std::vector rotateMode_; + const std::vector rotateData_; SampleTrajectoryImplCpu &sampleTrajectory_; SegCosts &segCosts_; size_t &optimizationAttempts_; @@ -1447,7 +1457,9 @@ double myfunc(const std::vector &x, std::vector &grad, void *f_d nloptParams->trajectory_, nloptParams->pathToMapTransform_, nloptParams->sampleTrajectory_, - nloptParams->optParams_)) + nloptParams->optParams_, + nloptParams->rotateMode_, + nloptParams->rotateData_)) { ROS_ERROR_STREAM("base_trajectory_node : RPROP initial evaluateTrajectory() falied"); return std::numeric_limits::quiet_NaN(); @@ -1472,6 +1484,8 @@ bool NLOPT( OptParamsList &optParams, const std::vector &jointNames, const geometry_msgs::TransformStamped &pathToMapTransform, + const std::vector &rotateMode, + const std::vector &rotateData, SampleTrajectoryImplCpu &sampleTrajectory) { for (const auto &it: optParams) @@ -1494,6 +1508,8 @@ bool NLOPT( optParams, jointNames, pathToMapTransform, + rotateMode, + rotateData, sampleTrajectory, segCosts, optimizationAttempts); @@ -1978,6 +1994,29 @@ bool callback(base_trajectory_msgs::GenerateSpline::Request &msg, } kinematicConstraints.addConstraints(msg.constraints); + + for (size_t i = 0; i < msg.rotate_mode.size(); i++) + { + if (msg.rotate_mode[i] == base_trajectory_msgs::GenerateSpline::Request::ROTATE_MODE_FIXED_COORDINATE) + { + if (msg.rotate_data[i].header.frame_id == "map") + { + if (!transformConstraintPoint(msg.rotate_data[i].point, inversePathToMapTransform)) + { + return false; + } + } + else + { + if (!transformConstraintPoint(msg.rotate_data[i].point, msg.rotate_data[i].header, pathFrameID)) + { + return false; + } + } + } + } + + OptParamsList optParams; for (const auto &pol : msg.path_offset_limit) { @@ -2027,27 +2066,27 @@ bool callback(base_trajectory_msgs::GenerateSpline::Request &msg, std::vector equalArcLengthTimes; std::vector remappedTimes; std::vector vTrans; - if (!evaluateTrajectory(cost, segCosts, equalArcLengthTimes, remappedTimes, vTrans, trajectory, pathToMapTransform, sampleTrajectory, optParams)) + if (!evaluateTrajectory(cost, segCosts, equalArcLengthTimes, remappedTimes, vTrans, trajectory, pathToMapTransform, sampleTrajectory, optParams, msg.rotate_mode, msg.rotate_data)) { ROS_ERROR_STREAM("base_trajectory_node : evaluateTrajectory() returned false"); return false; } base_trajectory_msgs::GenerateSpline::Response tmp_msg; - trajectoryToSplineResponseMsg(tmp_msg, trajectory, jointNames, pathToMapTransform, sampleTrajectory, optParams); + trajectoryToSplineResponseMsg(tmp_msg, trajectory, jointNames, pathToMapTransform, sampleTrajectory, optParams, msg.rotate_mode, msg.rotate_data); writeMatlabSplines(trajectory, 1, "Initial Splines"); messageFilter.disable(); fflush(stdout); // Call optimization, get optimizated result in trajectory #if 0 - if (!RPROP(trajectory, msg.points, optParams, jointNames, pathToMapTransform, sampleTrajectory)) + if (!RPROP(trajectory, msg.points, optParams, jointNames, pathToMapTransform, msg.rotate_mode, msg.rotate_data, sampleTrajectory)) { ROS_ERROR_STREAM("base_trajectory_node : RPROP() returned false"); return false; } #else - if (!NLOPT(trajectory, msg.points, optParams, jointNames, pathToMapTransform, sampleTrajectory)) + if (!NLOPT(trajectory, msg.points, optParams, jointNames, pathToMapTransform, msg.rotate_mode, msg.rotate_data, sampleTrajectory)) { ROS_ERROR_STREAM("base_trajectory_node : NLOPT() returned false"); return false; @@ -2056,7 +2095,7 @@ bool callback(base_trajectory_msgs::GenerateSpline::Request &msg, messageFilter.enable(); } - trajectoryToSplineResponseMsg(out_msg, trajectory, jointNames, pathToMapTransform, sampleTrajectory, optParams); + trajectoryToSplineResponseMsg(out_msg, trajectory, jointNames, pathToMapTransform, sampleTrajectory, optParams, msg.rotate_mode, msg.rotate_data); writeMatlabSplines(trajectory, 2, "Optimized Splines"); fflush(stdout); const auto t2 = high_resolution_clock::now(); diff --git a/zebROS_ws/src/base_trajectory_msgs/CMakeLists.txt b/zebROS_ws/src/base_trajectory_msgs/CMakeLists.txt index 5e8011f90..97371abc2 100644 --- a/zebROS_ws/src/base_trajectory_msgs/CMakeLists.txt +++ b/zebROS_ws/src/base_trajectory_msgs/CMakeLists.txt @@ -12,7 +12,6 @@ find_package(catkin REQUIRED COMPONENTS nav_msgs std_msgs trajectory_msgs - field_obj ) ## System dependencies are found with CMake's conventions @@ -75,7 +74,6 @@ generate_messages( nav_msgs std_msgs trajectory_msgs - field_obj ) ## Generate messages in the 'msg' folder diff --git a/zebROS_ws/src/base_trajectory_msgs/package.xml b/zebROS_ws/src/base_trajectory_msgs/package.xml index 3c3c567b0..9204e0632 100644 --- a/zebROS_ws/src/base_trajectory_msgs/package.xml +++ b/zebROS_ws/src/base_trajectory_msgs/package.xml @@ -60,7 +60,6 @@ nav_msgs std_msgs trajectory_msgs - field_obj diff --git a/zebROS_ws/src/base_trajectory_msgs/srv/GenerateSpline.srv b/zebROS_ws/src/base_trajectory_msgs/srv/GenerateSpline.srv index f1cf58c75..187d6354c 100644 --- a/zebROS_ws/src/base_trajectory_msgs/srv/GenerateSpline.srv +++ b/zebROS_ws/src/base_trajectory_msgs/srv/GenerateSpline.srv @@ -4,6 +4,14 @@ string[] point_frame_id PathOffsetLimit[] path_offset_limit bool optimize_final_velocity Constraint[] constraints + +int8 ROTATE_MODE_VALUE=0 +int8 ROTATE_MODE_FIXED_COORDINATE=1 +int8 ROTATE_MODE_NEXT_WAYPOINT=2 +int8 ROTATE_MODE_CONSTANT_VELOCITY=3 + +int8[] rotate_mode +geometry_msgs/PointStamped[] rotate_data --- Coefs[] orient_coefs Coefs[] x_coefs From 7f9aa41b5413a94d08a78793e15b82d3e49fb046 Mon Sep 17 00:00:00 2001 From: Kevin Jaget Date: Mon, 12 Sep 2022 13:33:01 -0400 Subject: [PATCH 2/3] More plumbing work, start of generateRotationState code --- .../base_trajectory/sample_trajectory.h | 5 ++- .../base_trajectory/sample_trajectory_impl.h | 5 ++- .../sample_trajectory_impl_cpu.h | 8 +++- .../sample_trajectory_impl_cuda.h | 4 +- .../sample_trajectory_impl_hybrid.h | 4 +- .../sample_trajectory_impl_threaded.h | 4 +- .../base_trajectory/src/base_trajectory.cpp | 3 +- .../base_trajectory/src/sample_trajectory.cpp | 8 +++- .../src/sample_trajectory_impl_cpu.cpp | 39 +++++++++++++++++-- .../src/sample_trajectory_impl_cuda.cu | 4 +- .../src/sample_trajectory_impl_threaded.cpp | 4 +- .../srv/GenerateSpline.srv | 2 +- 12 files changed, 74 insertions(+), 16 deletions(-) diff --git a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory.h b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory.h index 8c5b41aeb..a687deeb4 100644 --- a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory.h +++ b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory.h @@ -1,6 +1,7 @@ #ifndef INC_SAMPLE_TRAJECTORY_ #define INC_SAMPLE_TRAJECTORY_ +#include "geometry_msgs/PointStamped.h" #include "spline_util/spline_util.h" enum class SampleTrajectoryType @@ -29,7 +30,9 @@ class SampleTrajectory std::vector> &yStates, std::vector> &thetaStates, const XYTTrajectory &trajectory, - const ArcLengthTrajectory &arcLengthTrajectory); + const ArcLengthTrajectory &arcLengthTrajectory, + const std::vector &rotateMode, + const std::vector &rotateData); void setDistBetweenArcLengths(double distBetweenArcLength); diff --git a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl.h b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl.h index e2a78c2ec..e0cb05fde 100644 --- a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl.h +++ b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl.h @@ -1,6 +1,7 @@ #ifndef INC_SAMPLE_TRAJECTORY_IMPL_ #define INC_SAMPLE_TRAJECTORY_IMPL_ +#include "geometry_msgs/PointStamped.h" #include "spline_util/spline_util.h" template @@ -15,7 +16,9 @@ class SampleTrajectoryImpl std::vector> &yStates, std::vector> &thetaStates, const XYTTrajectory &trajectory, - const ArcLengthTrajectory &arcLengthTrajectory) = 0; + const ArcLengthTrajectory &arcLengthTrajectory, + const std::vector &rotateMode, + const std::vector &rotateData) = 0; virtual void setDistBetweenArcLengths(double distBetweenArcLength); virtual void setDistBetweenArcLengthEpsilon(double distBetweenArcLengthEpsilon); diff --git a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_cpu.h b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_cpu.h index b8b9c4098..5bd25c81d 100644 --- a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_cpu.h +++ b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_cpu.h @@ -16,7 +16,9 @@ class SampleTrajectoryImplCpu : public SampleTrajectoryImpl std::vector> &yStates, std::vector> &tStates, const XYTTrajectory &trajectory, - const ArcLengthTrajectory &arcLengthTrajectory) override; + const ArcLengthTrajectory &arcLengthTrajectory, + const std::vector &rotateMode, + const std::vector &rotateData) override; protected: bool subdivideLength(std::vector &equalArcLengthTimes, std::vector &equalArcLengthPositions, @@ -25,7 +27,9 @@ class SampleTrajectoryImplCpu : public SampleTrajectoryImpl std::vector> &yStates, std::vector> &tStates, // thetaState == rotation state const std::vector &equalArcLengthTimes, - const XYTTrajectory &trajectory); + const XYTTrajectory &trajectory, + const std::vector &rotateMode, + const std::vector &rotateData); }; #endif diff --git a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_cuda.h b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_cuda.h index 720110585..fea344feb 100644 --- a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_cuda.h +++ b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_cuda.h @@ -22,7 +22,9 @@ class SampleTrajectoryImplCuda : public SampleTrajectoryImpl std::vector> &yStates, std::vector> &tStates, const XYTTrajectory &trajectory, - const ArcLengthTrajectory &arcLengthTrajectory) override; + const ArcLengthTrajectory &arcLengthTrajectory, + const std::vector &rotateMode, + const std::vector &rotateData) override; private: cudaStream_t findArcLengthStream_; cudaStream_t sampleXYTStream_; diff --git a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_hybrid.h b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_hybrid.h index f0f7a41fb..03e3fad12 100644 --- a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_hybrid.h +++ b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_hybrid.h @@ -20,7 +20,9 @@ class SampleTrajectoryImplHybrid : public SampleTrajectoryImplCpu std::vector> &yStates, std::vector> &tStates, const XYTTrajectory &trajectory, - const ArcLengthTrajectory &arcLengthTrajectory) override; + const ArcLengthTrajectory &arcLengthTrajectory, + const std::vector &rotateMode, + const std::vector &rotateData) override; private: cudaStream_t sampleXYTStream_; diff --git a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_threaded.h b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_threaded.h index 2cdf64aaf..95cae3ffd 100644 --- a/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_threaded.h +++ b/zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_threaded.h @@ -17,7 +17,9 @@ class SampleTrajectoryImplThreaded : public SampleTrajectoryImpl std::vector> &yStates, std::vector> &tStates, const XYTTrajectory &trajectory, - const ArcLengthTrajectory &arcLengthTrajectory) override; + const ArcLengthTrajectory &arcLengthTrajectory, + const std::vector &rotateMode, + const std::vector &rotateData) override; private: bool subdivideLength(std::vector &equalArcLengthTimes, std::vector &equalArcLengthPositions, diff --git a/zebROS_ws/src/base_trajectory/src/base_trajectory.cpp b/zebROS_ws/src/base_trajectory/src/base_trajectory.cpp index ddc3be538..8045e1e7a 100644 --- a/zebROS_ws/src/base_trajectory/src/base_trajectory.cpp +++ b/zebROS_ws/src/base_trajectory/src/base_trajectory.cpp @@ -962,7 +962,8 @@ bool evaluateTrajectory(T &cost, static std::vector> tStates; if (!sampleTrajectory.sample(equalArcLengthTimes, equalArcLengthPositions, xStates, yStates, tStates, - trajectory, arcLengthTrajectory)) + trajectory, arcLengthTrajectory, + rotateMode, rotateData)) { ROS_ERROR_STREAM("evaluateTrajectory :: sample() failed"); return false; diff --git a/zebROS_ws/src/base_trajectory/src/sample_trajectory.cpp b/zebROS_ws/src/base_trajectory/src/sample_trajectory.cpp index 25fa0f997..6550b8297 100644 --- a/zebROS_ws/src/base_trajectory/src/sample_trajectory.cpp +++ b/zebROS_ws/src/base_trajectory/src/sample_trajectory.cpp @@ -29,11 +29,15 @@ bool SampleTrajectory::sample(std::vector &equalArcLengthTimes, std::vector> &yStates, std::vector> &thetaStates, const XYTTrajectory &trajectory, - const ArcLengthTrajectory &arcLengthTrajectory) + const ArcLengthTrajectory &arcLengthTrajectory, + const std::vector &rotateMode, + const std::vector &rotateData) { return impl_->sample(equalArcLengthTimes, equalArcLengthPositions, xStates, yStates, thetaStates, - trajectory, arcLengthTrajectory); + trajectory, arcLengthTrajectory, + const std::vector &rotateMode, + const std::vector &rotateData); } template diff --git a/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cpu.cpp b/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cpu.cpp index b880e640a..c7dfebaed 100644 --- a/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cpu.cpp +++ b/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cpu.cpp @@ -16,10 +16,12 @@ bool SampleTrajectoryImplCpu::sample(std::vector &equalArcLengthTimes, std::vector> &yStates, std::vector> &tStates, const XYTTrajectory &trajectory, - const ArcLengthTrajectory &arcLengthTrajectory) + const ArcLengthTrajectory &arcLengthTrajectory, + const std::vector &rotateMode, + const std::vector &rotateData) { return subdivideLength(equalArcLengthTimes, equalArcLengthPositions, arcLengthTrajectory) && - sampleEqualArcLengths(xStates, yStates, tStates, equalArcLengthTimes, trajectory); + sampleEqualArcLengths(xStates, yStates, tStates, equalArcLengthTimes, trajectory, rotateMode, rotateData); } template @@ -112,12 +114,43 @@ bool SampleTrajectoryImplCpu::subdivideLength(std::vector &equalArcLengthT return true; } +template +SegmentState generateRotationState( + const T t, // t is arbTime + const std::vector> &xStates, + const std::vector> &yStates) +{ + static SegmentState tState; + const auto seg = arcLengthTimeToSegTime(equalArcLengthTimes, i); + switch(rotate_mode) + { + case base_trajectory_msgs::GenerateSpline::Request::ROTATE_MODE_SPLINE: + auto tIt = trajectory[2].cbegin() + seg; + if (tIt >= trajectory[2].cend()) + { + ROS_ERROR_STREAM("base_trajectory : evaluateTrajectory could not sample tState at time " << t << ", seg = " << seg << ", trajectory[2].size() = " << trajectory[2].size()); + return false; + } + tIt->sample(t, tState); + break; + case base_trajectory_msgs::GenerateSpline::Request::ROTATE_MODE_FIXED_COORDINATE: + break; + case base_trajectory_msgs::GenerateSpline::Request::ROTATE_NEXT_WAYPOINT: + break; + case base_trajectory_msgs::GenerateSpline::Request::ROTATE_MODE_CONSTANT_VELOCITY: + break; + } + return tState; +} + template bool SampleTrajectoryImplCpu::sampleEqualArcLengths(std::vector> &xStates, std::vector> &yStates, std::vector> &tStates, // thetaState == rotation state const std::vector &equalArcLengthTimes, - const XYTTrajectory &trajectory) + const XYTTrajectory &trajectory, + const std::vector &rotateMode, + const std::vector &rotateData) { xStates.resize(equalArcLengthTimes.size()); yStates.resize(equalArcLengthTimes.size()); diff --git a/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cuda.cu b/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cuda.cu index a6a2566f7..a7b8acd5d 100644 --- a/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cuda.cu +++ b/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cuda.cu @@ -225,7 +225,9 @@ bool SampleTrajectoryImplCuda::sample(std::vector &equalArcLengthTimes, std::vector> &yStates, std::vector> &tStates, const XYTTrajectory &trajectory, - const ArcLengthTrajectory &arcLengthTrajectory) + const ArcLengthTrajectory &arcLengthTrajectory, + const std::vector &rotateMode, + const std::vector &rotateData) { nvtxRangePushA(__FUNCTION__); // add a top level function tag diff --git a/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_threaded.cpp b/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_threaded.cpp index 1ba045b7f..ba9d85ac9 100644 --- a/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_threaded.cpp +++ b/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_threaded.cpp @@ -20,7 +20,9 @@ bool SampleTrajectoryImplThreaded::sample(std::vector &equalArcLengthTimes std::vector> &yStates, std::vector> &tStates, const XYTTrajectory &trajectory, - const ArcLengthTrajectory &arcLengthTrajectory) + const ArcLengthTrajectory &arcLengthTrajectory, + const std::vector &rotateMode, + const std::vector &rotateData) { return subdivideLength(equalArcLengthTimes, equalArcLengthPositions, arcLengthTrajectory) && sampleEqualArcLengths(xStates, yStates, tStates, equalArcLengthTimes, trajectory); diff --git a/zebROS_ws/src/base_trajectory_msgs/srv/GenerateSpline.srv b/zebROS_ws/src/base_trajectory_msgs/srv/GenerateSpline.srv index 187d6354c..194217e14 100644 --- a/zebROS_ws/src/base_trajectory_msgs/srv/GenerateSpline.srv +++ b/zebROS_ws/src/base_trajectory_msgs/srv/GenerateSpline.srv @@ -5,7 +5,7 @@ PathOffsetLimit[] path_offset_limit bool optimize_final_velocity Constraint[] constraints -int8 ROTATE_MODE_VALUE=0 +int8 ROTATE_MODE_SPLINE=0 int8 ROTATE_MODE_FIXED_COORDINATE=1 int8 ROTATE_MODE_NEXT_WAYPOINT=2 int8 ROTATE_MODE_CONSTANT_VELOCITY=3 From 0fed95f5b90cc942be997a03522de948e36bb956 Mon Sep 17 00:00:00 2001 From: Kevin Jaget Date: Sun, 16 Oct 2022 20:20:04 -0400 Subject: [PATCH 3/3] More work in progress --- .../src/sample_trajectory_impl_cpu.cpp | 44 +++++++++++-------- 1 file changed, 25 insertions(+), 19 deletions(-) diff --git a/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cpu.cpp b/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cpu.cpp index c7dfebaed..340de613b 100644 --- a/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cpu.cpp +++ b/zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cpu.cpp @@ -115,23 +115,19 @@ bool SampleTrajectoryImplCpu::subdivideLength(std::vector &equalArcLengthT } template -SegmentState generateRotationState( - const T t, // t is arbTime - const std::vector> &xStates, - const std::vector> &yStates) +bool generateExtraRotationState( + SegmentState &tState, + const size_t index, + int8_t rotate_mode, + const std::vector> &xStates, + const std::vector> &yStates, + const std::vector &rotateData, +) { - static SegmentState tState; - const auto seg = arcLengthTimeToSegTime(equalArcLengthTimes, i); switch(rotate_mode) { case base_trajectory_msgs::GenerateSpline::Request::ROTATE_MODE_SPLINE: - auto tIt = trajectory[2].cbegin() + seg; - if (tIt >= trajectory[2].cend()) - { - ROS_ERROR_STREAM("base_trajectory : evaluateTrajectory could not sample tState at time " << t << ", seg = " << seg << ", trajectory[2].size() = " << trajectory[2].size()); - return false; - } - tIt->sample(t, tState); + // do nothing - tState was already sampled from trajectories in first pass break; case base_trajectory_msgs::GenerateSpline::Request::ROTATE_MODE_FIXED_COORDINATE: break; @@ -139,8 +135,10 @@ SegmentState generateRotationState( break; case base_trajectory_msgs::GenerateSpline::Request::ROTATE_MODE_CONSTANT_VELOCITY: break; + default: + return false; } - return tState; + return true; } template @@ -155,6 +153,7 @@ bool SampleTrajectoryImplCpu::sampleEqualArcLengths(std::vector::sampleEqualArcLengths(std::vectorsample(t, yStates[i]); - auto tIt = trajectory[2].cbegin() + seg; - if (tIt >= trajectory[2].cend()) + if (rotate_mode[seg] == base_trajectory_msgs::GenerateSpline::Request::ROTATE_MODE_SPLINE) { - ROS_ERROR_STREAM("base_trajectory : evaluateTrajectory could not sample tState at time " << t << ", seg = " << seg << ", trajectory[2].size() = " << trajectory[2].size()); - return false; + auto tIt = trajectory[2].cbegin() + seg; + if (tIt >= trajectory[2].cend()) + { + ROS_ERROR_STREAM("base_trajectory : evaluateTrajectory could not sample tState at time " << t << ", seg = " << seg << ", trajectory[2].size() = " << trajectory[2].size()); + return false; + } + tIt->sample(t, tStates[i]); + } + else + { + needTwoPasses = true; } - tIt->sample(t, tStates[i]); } return true; }