Note: The default ITS GitLab runner is a shared resource and is subject to slowdowns during heavy usage.
You can run your own GitLab runner that is dedicated just to your group if you need to avoid processing delays.

Commit 14976439 authored by Dushyanth Ganesan's avatar Dushyanth Ganesan
Browse files

fixed data point collection, added paths count and appended save.bash

parent a9d73be7
......@@ -10,5 +10,12 @@ pushd ~/data/vna2v/finaldata > /dev/null
read -p "Enter name of simulation run (HEIGHT_SKIPPIXEL): " name
cp ~/.ros/time.txt ./"${name}_time.txt"
cp ~/.ros/cost.txt ./"${name}_cost.txt"
cp ~/.ros/paths.txt ./"${name}_paths.txt"
printf "Number of time data points: "
cat ~/.ros/time.txt | wc -l
printf "Number of cost data points: "
cat ~/.ros/cost.txt | wc -l
printf "Number of path data points: "
cat ~/.ros/paths.txt | wc -l
popd > /dev/null
echo "Saved ${name}_time.txt and ${name}_cost.txt to ~/data/vna2v/finaldata"
\ No newline at end of file
......@@ -59,7 +59,8 @@ public:
void setEnvironment(const EDTEnvironment::Ptr& env);
void setParam(ros::NodeHandle& nh);
Eigen::MatrixXd BsplineOptimizeTraj(const Eigen::MatrixXd& points, const double& ts,
const int& cost_function, int max_num_id, int max_time_id);
const int& cost_function, int max_num_id, int max_time_id,
bool printCost = false);
/* helper function */
......@@ -74,7 +75,7 @@ public:
void setWaypoints(const vector<Eigen::Vector3d>& waypts,
const vector<int>& waypt_idx); // N-2 constraints at most
void optimize();
void optimize(bool printCosts);
Eigen::MatrixXd getControlPoints();
vector<Eigen::Vector3d> matrixToVectors(const Eigen::MatrixXd& ctrl_pts);
......
......@@ -116,17 +116,17 @@ void BsplineOptimizer::setWaypoints(const vector<Eigen::Vector3d>& waypts,
Eigen::MatrixXd BsplineOptimizer::BsplineOptimizeTraj(const Eigen::MatrixXd& points, const double& ts,
const int& cost_function, int max_num_id,
int max_time_id) {
int max_time_id, bool printCost) {
setControlPoints(points);
setBsplineInterval(ts);
setCostFunction(cost_function);
setTerminateCond(max_num_id, max_time_id);
optimize();
optimize(printCost);
return this->control_points_;
}
void BsplineOptimizer::optimize() {
void BsplineOptimizer::optimize(bool printCosts) {
/* initialize solver */
iter_num_ = 0;
min_cost_ = std::numeric_limits<double>::max();
......@@ -186,8 +186,10 @@ void BsplineOptimizer::optimize() {
/* retrieve the optimization result */
cout << "Minimum combined cost:" << final_cost << endl;
cout << "Writing combined cost to cost file" << endl;
costFile << final_cost << "\n";
if (printCosts) {
cout << "Writing combined cost to cost file" << endl;
costFile << final_cost << "\n";
}
} catch (std::exception& e) {
ROS_WARN("[Optimization]: nlopt exception");
cout << e.what() << endl;
......
......@@ -29,6 +29,7 @@
#include <plan_env/edt_environment.h>
#include <plan_env/raycast.h>
#include <random>
#include <fstream>
namespace fast_planner {
......@@ -183,7 +184,7 @@ private:
public:
double clearance_;
ofstream pathFile;
TopologyPRM(/* args */);
~TopologyPRM();
......
......@@ -25,9 +25,12 @@
#include <path_searching/topo_prm.h>
#include <thread>
#include <fstream>
namespace fast_planner {
TopologyPRM::TopologyPRM(/* args */) {}
TopologyPRM::TopologyPRM(/* args */) {
pathFile.open("paths.txt", ios::out | ios::binary);
}
TopologyPRM::~TopologyPRM() {}
......@@ -94,7 +97,9 @@ void TopologyPRM::findTopoPaths(Eigen::Vector3d start, Eigen::Vector3d end,
t1 = ros::Time::now();
filtered_paths = pruneEquivalent(short_paths_);
////// PRINT THIS ////////////////////////////////////////////////////////////////
cout << "Number of paths found: " << filtered_paths.size() << endl;
pathFile << filtered_paths.size() << "\n";
prune_time = (ros::Time::now() - t1).toSec();
// cout << "prune: " << (t2 - t1).toSec() << endl;
......
......@@ -199,7 +199,7 @@ bool FastPlannerManager::kinodynamicReplan(Eigen::Vector3d start_pt, Eigen::Vect
cost_function |= BsplineOptimizer::ENDPOINT;
}
ctrl_pts = bspline_optimizers_[0]->BsplineOptimizeTraj(ctrl_pts, ts, cost_function, 1, 1);
ctrl_pts = bspline_optimizers_[0]->BsplineOptimizeTraj(ctrl_pts, ts, cost_function, 1, 1, true);
t_opt = (ros::Time::now() - t1).toSec();
......@@ -427,7 +427,7 @@ void FastPlannerManager::refineTraj(NonUniformBspline& best_traj, double& time_i
reparamBspline(best_traj, ratio, ctrl_pts, dt, t_inc);
time_inc += t_inc;
ctrl_pts = bspline_optimizers_[0]->BsplineOptimizeTraj(ctrl_pts, dt, cost_function, 1, 1);
ctrl_pts = bspline_optimizers_[0]->BsplineOptimizeTraj(ctrl_pts, dt, cost_function, 1, 1, true);
best_traj = NonUniformBspline(ctrl_pts, 3, dt);
ROS_WARN_STREAM("[Refine]: cost " << (ros::Time::now() - t1).toSec()
<< " seconds, time change is: " << time_inc);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment