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 f38c23e8b..8045e1e7a 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. @@ -960,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; @@ -1194,7 +1197,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 +1252,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 +1390,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 +1400,8 @@ struct NLOPTParams , optParams_(optParams) , jointNames_(jointNames) , pathToMapTransform_(pathToMapTransform) + , rotateMode_(rotateMode) + , rotateData_(rotateData) , sampleTrajectory_(sampleTrajectory) , segCosts_(segCosts) , optimizationAttempts_(optimizationAttempts) @@ -1406,6 +1415,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 +1458,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 +1485,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 +1509,8 @@ bool NLOPT( optParams, jointNames, pathToMapTransform, + rotateMode, + rotateData, sampleTrajectory, segCosts, optimizationAttempts); @@ -1978,6 +1995,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 +2067,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 +2096,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/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..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 @@ -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,16 +114,46 @@ bool SampleTrajectoryImplCpu::subdivideLength(std::vector &equalArcLengthT return true; } +template +bool generateExtraRotationState( + SegmentState &tState, + const size_t index, + int8_t rotate_mode, + const std::vector> &xStates, + const std::vector> &yStates, + const std::vector &rotateData, +) +{ + switch(rotate_mode) + { + case base_trajectory_msgs::GenerateSpline::Request::ROTATE_MODE_SPLINE: + // do nothing - tState was already sampled from trajectories in first pass + 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; + default: + return false; + } + return true; +} + 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()); tStates.resize(equalArcLengthTimes.size()); + bool needTwoPasses = false; for (size_t i = 0; i < equalArcLengthTimes.size(); i++) { const auto t = equalArcLengthTimes[i]; @@ -146,13 +178,20 @@ bool SampleTrajectoryImplCpu::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; } 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/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..194217e14 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_SPLINE=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