Remove 2D Trajectory

Author: haider8645

src/EnvironmentXYZTheta.cpp

@@ -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;

src/EnvironmentXYZTheta.hpp

@@ -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;
 

src/Planner.cpp

@@ -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)
 {
@@ -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();
 
@@ -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)
     {

src/Planner.hpp

@@ -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);
 

src/gui/PlannerGui.cpp

@@ -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);
@@ -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();
 
@@ -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;
@@ -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 *>()));
@@ -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:

src/gui/PlannerGui.h

@@ -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;    
 };