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

Remove 2d trajectory #29

Draft
wants to merge 2 commits into
base: main
Choose a base branch
from
Draft

Remove 2d trajectory #29

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a558a32
Remove 2D Trajectory
haider8645 Nov 26, 2024
7e2ec28
Update tests
haider8645 Nov 27, 2024
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
11 changes: 4 additions & 7 deletions src/EnvironmentXYZTheta.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -951,8 +951,10 @@ vector<Motion> EnvironmentXYZTheta::getMotions(const vector< int >& stateIDPath)

void EnvironmentXYZTheta::getTrajectory(const vector<int>& stateIDPath,
vector<SubTrajectory>& result,
bool setZToZero, const Eigen::Vector3d &startPos,
const Eigen::Vector3d &goalPos, const double& goalHeading, const Eigen::Affine3d &plan2Body)
const Eigen::Vector3d &startPos,
const Eigen::Vector3d &goalPos,
const double& goalHeading,
const Eigen::Affine3d &plan2Body)
{
if(stateIDPath.size() < 2)
return;
Expand Down Expand Up @@ -1014,11 +1016,6 @@ void EnvironmentXYZTheta::getTrajectory(const vector<int>& stateIDPath,
#ifdef ENABLE_DEBUG_DRAWINGS
V3DD::DRAW_SPHERE("ugv_nav4d_trajectory_poses", pointOnTravPlane, 0.01, V3DD::Color::red);
#endif
//TODO: Only left here until software which still uses trajectory2D is updated to use trajectory3D
if (setZToZero){
pointOnTravPlane.z() = 0;
}

Eigen::Vector3d pointOnBody = plan2Body.inverse(Eigen::Isometry) * pointOnTravPlane;
if (positions.empty() || !(positions.back().isApprox(pointOnBody)))
{
Expand Down
3 changes: 2 additions & 1 deletion src/EnvironmentXYZTheta.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -200,7 +200,8 @@ class EnvironmentXYZTheta : public DiscreteSpaceInformation
std::vector<Motion> getMotions(const std::vector<int> &stateIDPath);

void getTrajectory(const std::vector<int> &stateIDPath, std::vector<trajectory_follower::SubTrajectory> &result,
bool setZToZero, const Eigen::Vector3d &startPos, const Eigen::Vector3d &goalPos, const double& goalHeading, const Eigen::Affine3d &plan2Body = Eigen::Affine3d::Identity());
const Eigen::Vector3d &startPos, const Eigen::Vector3d &goalPos, const double& goalHeading,
const Eigen::Affine3d &plan2Body = Eigen::Affine3d::Identity());

const PreComputedMotions& getAvailableMotions() const;

Expand Down
5 changes: 1 addition & 4 deletions src/Planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,6 @@ bool Planner::tryGoal(const Eigen::Vector3d& translation, const double yaw) noex

Planner::PLANNING_RESULT Planner::plan(const base::Time& maxTime, const base::samples::RigidBodyState& start_pose,
const base::samples::RigidBodyState& end_pose,
std::vector<SubTrajectory>& resultTrajectory2D,
std::vector<SubTrajectory>& resultTrajectory3D,
bool dumpOnError, bool dumpOnSuccess)
{
Expand All @@ -150,7 +149,6 @@ Planner::PLANNING_RESULT Planner::plan(const base::Time& maxTime, const base::sa
return NO_MAP;
}

resultTrajectory2D.clear();
resultTrajectory3D.clear();
env->clear();

Expand Down Expand Up @@ -261,8 +259,7 @@ Planner::PLANNING_RESULT Planner::plan(const base::Time& maxTime, const base::sa
std::vector<PlannerStats> stats;

planner->get_search_stats(&stats);
env->getTrajectory(solutionIds, resultTrajectory2D, true, start_translation, goal_translation, end_pose.getYaw(), ground2Body);
env->getTrajectory(solutionIds, resultTrajectory3D, false, start_translation, goal_translation,end_pose.getYaw(), ground2Body);
env->getTrajectory(solutionIds, resultTrajectory3D, start_translation, goal_translation,end_pose.getYaw(), ground2Body);
}
catch(const SBPL_Exception& ex)
{
Expand Down
5 changes: 3 additions & 2 deletions src/Planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,8 +123,9 @@ class Planner
* @return An enum indicating the planner state
* */
PLANNING_RESULT plan(const base::Time& maxTime, const base::samples::RigidBodyState& start_pose,
const base::samples::RigidBodyState& end_pose, std::vector<trajectory_follower::SubTrajectory>& resultTrajectory2D,
std::vector<trajectory_follower::SubTrajectory>& resultTrajectory3D, bool dumpOnError = false, bool dumpOnSuccess = false);
const base::samples::RigidBodyState& end_pose,
std::vector<trajectory_follower::SubTrajectory>& resultTrajectory3D, bool dumpOnError = false,
bool dumpOnSuccess = false);

void setTravConfig(const traversability_generator3d::TraversabilityConfig& config);

Expand Down
18 changes: 5 additions & 13 deletions src/gui/PlannerGui.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,6 @@ void PlannerGui::setupUI()
widget->setCameraManipulator(vizkit3d::ORBIT_MANIPULATOR);
widget->addPlugin(&splineViz);
widget->addPlugin(&trajViz);
widget->addPlugin(&trajViz2);
widget->addPlugin(&mlsViz);
widget->addPlugin(&trav3dViz);
widget->addPlugin(&obstacleMapViz);
Expand All @@ -96,13 +95,9 @@ void PlannerGui::setupUI()
mlsViz.setPluginName("MLSMap");

trajViz.setLineWidth(5);
trajViz.setColor(QColor("Cyan"));
trajViz.setPluginEnabled(false);
trajViz.setPluginName("Trajectory 2D");

trajViz2.setLineWidth(5);
trajViz2.setColor(QColor("magenta"));
trajViz2.setPluginName("Trajectory 3D");
trajViz.setColor(QColor("magenta"));
trajViz.setPluginEnabled(true);
trajViz.setPluginName("Trajectory");

QVBoxLayout* layout = new QVBoxLayout();

Expand Down Expand Up @@ -324,7 +319,7 @@ void PlannerGui::setupPlanner(int argc, char** argv)
mobilityConfig.multiplierPointTurn = 3;
mobilityConfig.maxMotionCurveLength = 100;
mobilityConfig.spline_sampling_resolution = 0.05;
mobilityConfig.remove_goal_offset = false;
mobilityConfig.remove_goal_offset = true;

travConfig.gridResolution = res;
travConfig.maxSlope = 0.45; //40.0/180.0 * M_PI;
Expand Down Expand Up @@ -611,9 +606,6 @@ void PlannerGui::plannerIsDone()
{
trajViz.updateData(path);
trajViz.setLineWidth(8);

trajViz2.updateData(beautifiedPath);
trajViz2.setLineWidth(8);

trav3dViz.updateData((planner->getTraversabilityMap().copyCast<maps::grid::TraversabilityNodeBase *>()));
obstacleMapViz.updateData((planner->getObstacleMap().copyCast<maps::grid::TraversabilityNodeBase *>()));
Expand Down Expand Up @@ -655,7 +647,7 @@ void PlannerGui::plan(const base::Pose& start, const base::Pose& goal)
LOG_INFO_S << "Planning: " << start << " -> " << goal;

const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(time->value()),
startState, endState, path, beautifiedPath);
startState, endState, path);
switch(result)
{
case Planner::GOAL_INVALID:
Expand Down
4 changes: 1 addition & 3 deletions src/gui/PlannerGui.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,5 @@ private slots:
traversability_generator3d::TraversabilityConfig travConfig;
ugv_nav4d::PlannerConfig plannerConfig;
std::shared_ptr<ugv_nav4d::Planner> planner; //is pointer cause of lazy init
std::vector<trajectory_follower::SubTrajectory> path;
std::vector<trajectory_follower::SubTrajectory> beautifiedPath;

std::vector<trajectory_follower::SubTrajectory> path;
};
41 changes: 3 additions & 38 deletions src/test/test_Planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,10 +133,9 @@ BOOST_AUTO_TEST_CASE(check_planner_goal_invalid) {
endState.orientation = Eigen::Quaterniond(1,0,0,0);

int maxTime = 5;
std::vector<trajectory_follower::SubTrajectory> trajectory2D;
std::vector<trajectory_follower::SubTrajectory> trajectory3D;

const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D);
const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory3D);
BOOST_CHECK_EQUAL(result, Planner::GOAL_INVALID);
}

Expand All @@ -158,10 +157,9 @@ BOOST_AUTO_TEST_CASE(check_planner_start_invalid) {
endState.orientation = Eigen::Quaterniond(1,0,0,0);

int maxTime = 5;
std::vector<trajectory_follower::SubTrajectory> trajectory2D;
std::vector<trajectory_follower::SubTrajectory> trajectory3D;

const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D);
const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory3D);
BOOST_CHECK_EQUAL(result, Planner::START_INVALID);
}

Expand All @@ -183,20 +181,13 @@ BOOST_AUTO_TEST_CASE(check_planner_success) {
endState.orientation = Eigen::Quaterniond(1,0,0,0);

int maxTime = 5;
std::vector<trajectory_follower::SubTrajectory> trajectory2D;
std::vector<trajectory_follower::SubTrajectory> trajectory3D;

const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory2D, trajectory3D);
const Planner::PLANNING_RESULT result = planner->plan(base::Time::fromSeconds(maxTime), startState, endState, trajectory3D);
BOOST_CHECK_EQUAL(result, Planner::FOUND_SOLUTION);
BOOST_CHECK_GT(trajectory2D.size(), 0);
BOOST_CHECK_GT(trajectory3D.size(), 0);
BOOST_CHECK_EQUAL(trajectory2D.size(), trajectory3D.size());

//Start and Goal Positions
BOOST_REQUIRE(trajectory2D.front().startPose.position.isApprox(startState.position.head<2>(), 1e-6));
BOOST_REQUIRE(trajectory2D.back().goalPose.position.isApprox(endState.position.head<2>(), 1e-6));
BOOST_REQUIRE(trajectory2D.front().posSpline.getStartPoint().isApprox(startState.position, 1e-6));
BOOST_REQUIRE(trajectory2D.back().posSpline.getEndPoint().isApprox(endState.position, 1e-6));

BOOST_REQUIRE(trajectory3D.front().startPose.position.isApprox(startState.position.head<2>(), 1e-6));
BOOST_REQUIRE(trajectory3D.back().goalPose.position.isApprox(endState.position.head<2>(), 1e-6));
Expand All @@ -205,37 +196,11 @@ BOOST_AUTO_TEST_CASE(check_planner_success) {


//Start and Goal Orientations
BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory2D.front().startPose.orientation).getRad(),
base::Angle::fromRad(base::getYaw(startState.orientation)).getRad());
BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory2D.back().goalPose.orientation).getRad(),
base::Angle::fromRad(base::getYaw(endState.orientation)).getRad());

BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory3D.front().startPose.orientation).getRad(),
base::Angle::fromRad(base::getYaw(startState.orientation)).getRad());
BOOST_REQUIRE_EQUAL(base::Angle::fromRad(trajectory3D.back().goalPose.orientation).getRad(),
base::Angle::fromRad(base::getYaw(endState.orientation)).getRad());

for (auto& trajectory : trajectory2D){
BOOST_REQUIRE(trajectory.speed <= mobility.translationSpeed);
BOOST_REQUIRE(trajectory.speed <= mobility.rotationSpeed);

if (trajectory.driveMode != trajectory_follower::ModeTurnOnTheSpot){
BOOST_REQUIRE(trajectory.posSpline.isEmpty() == false);

BOOST_REQUIRE(trajectory.getCurvatureMax() <= mobility.minTurningRadius/2);
BOOST_REQUIRE(trajectory.posSpline.getCurveLength() <= mobility.maxMotionCurveLength);
}
else{
BOOST_REQUIRE(trajectory.orientationSpline.isEmpty() == false);

BOOST_CHECK_CLOSE_FRACTION(base::Angle::fromRad(trajectory.getIntermediatePoint(trajectory.orientationSpline.getStartParam()).orientation).getRad(),
base::Angle::fromRad(trajectory.startPose.orientation).getRad(), 1e-6);

BOOST_CHECK_CLOSE_FRACTION(base::Angle::fromRad(trajectory.getIntermediatePoint(trajectory.orientationSpline.getEndParam()).orientation).getRad(),
base::Angle::fromRad(trajectory.goalPose.orientation).getRad(), 1e-6);
}
}

for (auto& trajectory : trajectory3D){
BOOST_REQUIRE(trajectory.speed <= mobility.translationSpeed);
BOOST_REQUIRE(trajectory.speed <= mobility.rotationSpeed);
Expand Down