Skip to content
New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Pathgen look at #173

Draft
wants to merge 3 commits into
base: master
Choose a base branch
from
Draft

Pathgen look at #173

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5ffeb9b
Add look-at mode for generating paths
kjaget Sep 12, 2022
7f9aa41
More plumbing work, start of generateRotationState code
kjaget Sep 12, 2022
0fed95f
More work in progress
kjaget Oct 17, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -29,7 +30,9 @@ class SampleTrajectory
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &thetaStates,
const XYTTrajectory<T> &trajectory,
const ArcLengthTrajectory<T> &arcLengthTrajectory);
const ArcLengthTrajectory<T> &arcLengthTrajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData);


void setDistBetweenArcLengths(double distBetweenArcLength);
Expand Down
5 changes: 4 additions & 1 deletion zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 <class T>
Expand All @@ -15,7 +16,9 @@ class SampleTrajectoryImpl
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &thetaStates,
const XYTTrajectory<T> &trajectory,
const ArcLengthTrajectory<T> &arcLengthTrajectory) = 0;
const ArcLengthTrajectory<T> &arcLengthTrajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData) = 0;

virtual void setDistBetweenArcLengths(double distBetweenArcLength);
virtual void setDistBetweenArcLengthEpsilon(double distBetweenArcLengthEpsilon);
Expand Down
8 changes: 6 additions & 2 deletions zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_cpu.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,9 @@ class SampleTrajectoryImplCpu : public SampleTrajectoryImpl<T>
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &tStates,
const XYTTrajectory<T> &trajectory,
const ArcLengthTrajectory<T> &arcLengthTrajectory) override;
const ArcLengthTrajectory<T> &arcLengthTrajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData) override;
protected:
bool subdivideLength(std::vector<T> &equalArcLengthTimes,
std::vector<T> &equalArcLengthPositions,
Expand All @@ -25,7 +27,9 @@ class SampleTrajectoryImplCpu : public SampleTrajectoryImpl<T>
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &tStates, // thetaState == rotation state
const std::vector<T> &equalArcLengthTimes,
const XYTTrajectory<T> &trajectory);
const XYTTrajectory<T> &trajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData);
};

#endif
4 changes: 3 additions & 1 deletion zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_cuda.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,9 @@ class SampleTrajectoryImplCuda : public SampleTrajectoryImpl<T>
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &tStates,
const XYTTrajectory<T> &trajectory,
const ArcLengthTrajectory<T> &arcLengthTrajectory) override;
const ArcLengthTrajectory<T> &arcLengthTrajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData) override;
private:
cudaStream_t findArcLengthStream_;
cudaStream_t sampleXYTStream_;
Expand Down
4 changes: 3 additions & 1 deletion zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_hybrid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,9 @@ class SampleTrajectoryImplHybrid : public SampleTrajectoryImplCpu<T>
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &tStates,
const XYTTrajectory<T> &trajectory,
const ArcLengthTrajectory<T> &arcLengthTrajectory) override;
const ArcLengthTrajectory<T> &arcLengthTrajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData) override;
private:
cudaStream_t sampleXYTStream_;

Expand Down
4 changes: 3 additions & 1 deletion zebROS_ws/src/base_trajectory/include/base_trajectory/sample_trajectory_impl_threaded.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,9 @@ class SampleTrajectoryImplThreaded : public SampleTrajectoryImpl<T>
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &tStates,
const XYTTrajectory<T> &trajectory,
const ArcLengthTrajectory<T> &arcLengthTrajectory) override;
const ArcLengthTrajectory<T> &arcLengthTrajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData) override;
private:
bool subdivideLength(std::vector<T> &equalArcLengthTimes,
std::vector<T> &equalArcLengthPositions,
Expand Down
60 changes: 50 additions & 10 deletions zebROS_ws/src/base_trajectory/src/base_trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -942,7 +942,9 @@ bool evaluateTrajectory(T &cost,
const XYTTrajectory<T> &trajectory,
const geometry_msgs::TransformStamped &pathToMapTransform,
SampleTrajectoryImplCpu<T> &sampleTrajectory,
const OptParamsList<T> &optParams)
const OptParamsList<T> &optParams,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData)
{
// arcLengthTrajectory takes in a time and returns the x-y-theta distance
// traveled up to that time.
Expand All @@ -960,7 +962,8 @@ bool evaluateTrajectory(T &cost,
static std::vector<SegmentState<T>> tStates;
if (!sampleTrajectory.sample(equalArcLengthTimes, equalArcLengthPositions,
xStates, yStates, tStates,
trajectory, arcLengthTrajectory))
trajectory, arcLengthTrajectory,
rotateMode, rotateData))
{
ROS_ERROR_STREAM("evaluateTrajectory :: sample() failed");
return false;
Expand Down Expand Up @@ -1194,7 +1197,9 @@ void trajectoryToSplineResponseMsg(base_trajectory_msgs::GenerateSpline::Respons
const std::vector<std::string> &jointNames,
const geometry_msgs::TransformStamped &pathToMapTransform,
SampleTrajectoryImplCpu<T> &sampleTrajectory,
const OptParamsList<T> &optParams)
const OptParamsList<T> &optParams,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData)
{
out_msg.orient_coefs.resize(trajectory[0].size());
out_msg.x_coefs.resize(trajectory[0].size());
Expand Down Expand Up @@ -1247,7 +1252,7 @@ void trajectoryToSplineResponseMsg(base_trajectory_msgs::GenerateSpline::Respons
std::vector<T> equalArcLengthTimes;
std::vector<T> remappedTimes;
std::vector<T> 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;
Expand Down Expand Up @@ -1385,6 +1390,8 @@ struct NLOPTParams
OptParamsList<T> &optParams,
const std::vector<std::string> &jointNames,
const geometry_msgs::TransformStamped &pathToMapTransform,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData,
SampleTrajectoryImplCpu<T> &sampleTrajectory,
SegCosts<T> &segCosts,
size_t &optimizationAttempts)
Expand All @@ -1393,6 +1400,8 @@ struct NLOPTParams
, optParams_(optParams)
, jointNames_(jointNames)
, pathToMapTransform_(pathToMapTransform)
, rotateMode_(rotateMode)
, rotateData_(rotateData)
, sampleTrajectory_(sampleTrajectory)
, segCosts_(segCosts)
, optimizationAttempts_(optimizationAttempts)
Expand All @@ -1406,6 +1415,8 @@ struct NLOPTParams
OptParamsList<T> &optParams_;
const std::vector<std::string> &jointNames_;
const geometry_msgs::TransformStamped &pathToMapTransform_;
const std::vector<int8_t> rotateMode_;
const std::vector<geometry_msgs::PointStamped> rotateData_;
SampleTrajectoryImplCpu<T> &sampleTrajectory_;
SegCosts<T> &segCosts_;
size_t &optimizationAttempts_;
Expand Down Expand Up @@ -1447,7 +1458,9 @@ double myfunc(const std::vector<double> &x, std::vector<double> &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<double>::quiet_NaN();
Expand All @@ -1472,6 +1485,8 @@ bool NLOPT(
OptParamsList<T> &optParams,
const std::vector<std::string> &jointNames,
const geometry_msgs::TransformStamped &pathToMapTransform,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData,
SampleTrajectoryImplCpu<T> &sampleTrajectory)
{
for (const auto &it: optParams)
Expand All @@ -1494,6 +1509,8 @@ bool NLOPT(
optParams,
jointNames,
pathToMapTransform,
rotateMode,
rotateData,
sampleTrajectory,
segCosts,
optimizationAttempts);
Expand Down Expand Up @@ -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<TrajectoryPointType> optParams;
for (const auto &pol : msg.path_offset_limit)
{
Expand Down Expand Up @@ -2027,27 +2067,27 @@ bool callback(base_trajectory_msgs::GenerateSpline::Request &msg,
std::vector<TrajectoryPointType> equalArcLengthTimes;
std::vector<TrajectoryPointType> remappedTimes;
std::vector<TrajectoryPointType> 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;
Expand All @@ -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();
Expand Down
8 changes: 6 additions & 2 deletions zebROS_ws/src/base_trajectory/src/sample_trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,11 +29,15 @@ bool SampleTrajectory<T>::sample(std::vector<T> &equalArcLengthTimes,
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &thetaStates,
const XYTTrajectory<T> &trajectory,
const ArcLengthTrajectory<T> &arcLengthTrajectory)
const ArcLengthTrajectory<T> &arcLengthTrajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData)
{
return impl_->sample(equalArcLengthTimes, equalArcLengthPositions,
xStates, yStates, thetaStates,
trajectory, arcLengthTrajectory);
trajectory, arcLengthTrajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData);
}

template <class T>
Expand Down
55 changes: 47 additions & 8 deletions zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cpu.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,10 +16,12 @@ bool SampleTrajectoryImplCpu<T>::sample(std::vector<T> &equalArcLengthTimes,
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &tStates,
const XYTTrajectory<T> &trajectory,
const ArcLengthTrajectory<T> &arcLengthTrajectory)
const ArcLengthTrajectory<T> &arcLengthTrajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData)
{
return subdivideLength(equalArcLengthTimes, equalArcLengthPositions, arcLengthTrajectory) &&
sampleEqualArcLengths(xStates, yStates, tStates, equalArcLengthTimes, trajectory);
sampleEqualArcLengths(xStates, yStates, tStates, equalArcLengthTimes, trajectory, rotateMode, rotateData);
}

template <class T>
Expand Down Expand Up @@ -112,16 +114,46 @@ bool SampleTrajectoryImplCpu<T>::subdivideLength(std::vector<T> &equalArcLengthT
return true;
}

template <class T>
bool generateExtraRotationState(
SegmentState<T> &tState,
const size_t index,
int8_t rotate_mode,
const std::vector<SegmentState<T>> &xStates,
const std::vector<SegmentState<T>> &yStates,
const std::vector<geometry_msgs::PointStamped> &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 <class T>
bool SampleTrajectoryImplCpu<T>::sampleEqualArcLengths(std::vector<SegmentState<T>> &xStates,
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &tStates, // thetaState == rotation state
const std::vector<T> &equalArcLengthTimes,
const XYTTrajectory<T> &trajectory)
const XYTTrajectory<T> &trajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &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];
Expand All @@ -146,13 +178,20 @@ bool SampleTrajectoryImplCpu<T>::sampleEqualArcLengths(std::vector<SegmentState<
}
yIt->sample(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;
}
Expand Down
4 changes: 3 additions & 1 deletion zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_cuda.cu
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -225,7 +225,9 @@ bool SampleTrajectoryImplCuda<T>::sample(std::vector<T> &equalArcLengthTimes,
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &tStates,
const XYTTrajectory<T> &trajectory,
const ArcLengthTrajectory<T> &arcLengthTrajectory)
const ArcLengthTrajectory<T> &arcLengthTrajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData)
{
nvtxRangePushA(__FUNCTION__); // add a top level function tag

Expand Down
4 changes: 3 additions & 1 deletion zebROS_ws/src/base_trajectory/src/sample_trajectory_impl_threaded.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,9 @@ bool SampleTrajectoryImplThreaded<T>::sample(std::vector<T> &equalArcLengthTimes
std::vector<SegmentState<T>> &yStates,
std::vector<SegmentState<T>> &tStates,
const XYTTrajectory<T> &trajectory,
const ArcLengthTrajectory<T> &arcLengthTrajectory)
const ArcLengthTrajectory<T> &arcLengthTrajectory,
const std::vector<int8_t> &rotateMode,
const std::vector<geometry_msgs::PointStamped> &rotateData)
{
return subdivideLength(equalArcLengthTimes, equalArcLengthPositions, arcLengthTrajectory) &&
sampleEqualArcLengths(xStates, yStates, tStates, equalArcLengthTimes, trajectory);
Expand Down
Loading