m6wf5oeyl
/
Enjoy
1
0
Fork 0
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity

socket add

Browse Source
pull/16/head
cys 5 months ago
parent ba53d6fbc2
commit 7d2d2732af
636 changed files with 538 additions and 100443 deletions
Show Stats Download Patch File Download Diff File

69
src/UAV/Modules/ego_planner/ego-planner-swarm/src/CMakeLists.txt
Unescape View File

@ -1,69 +0,0 @@
# toplevel CMakeLists.txt for a catkin workspace
# catkin/cmake/toplevel.cmake
cmake_minimum_required(VERSION 3.0.2)
project(Project)
set(CATKIN_TOPLEVEL TRUE)
# search for catkin within the workspace
set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
execute_process(COMMAND ${_cmd}
RESULT_VARIABLE _res
OUTPUT_VARIABLE _out
ERROR_VARIABLE _err
OUTPUT_STRIP_TRAILING_WHITESPACE
ERROR_STRIP_TRAILING_WHITESPACE
)
if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
# searching fot catkin resulted in an error
string(REPLACE ";" " " _cmd_str "${_cmd}")
message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
endif()
# include catkin from workspace or via find_package()
if(_res EQUAL 0)
set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
# include all.cmake without add_subdirectory to let it operate in same scope
include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
add_subdirectory("${_out}")
else()
# use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
# or CMAKE_PREFIX_PATH from the environment
if(NOT DEFINED CMAKE_PREFIX_PATH)
if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
if(NOT WIN32)
string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
else()
set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
endif()
endif()
endif()
# list of catkin workspaces
set(catkin_search_path "")
foreach(path ${CMAKE_PREFIX_PATH})
if(EXISTS "${path}/.catkin")
list(FIND catkin_search_path ${path} _index)
if(_index EQUAL -1)
list(APPEND catkin_search_path ${path})
endif()
endif()
endforeach()
# search for catkin in all workspaces
set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
find_package(catkin QUIET
NO_POLICY_SCOPE
PATHS ${catkin_search_path}
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
if(NOT catkin_FOUND)
message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
endif()
endif()
catkin_workspace()

45
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/bspline_opt/CMakeLists.txt
Unescape View File

@ -1,45 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(bspline_opt)
set(CMAKE_BUILD_TYPE "Release")
ADD_COMPILE_OPTIONS(-std=c++11 )
ADD_COMPILE_OPTIONS(-std=c++14 )
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
find_package(Eigen3 REQUIRED)
find_package(PCL 1.7 REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
visualization_msgs
plan_env
cv_bridge
traj_utils
path_searching
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES bspline_opt
CATKIN_DEPENDS plan_env
# DEPENDS system_lib
)
include_directories(
SYSTEM
include
${catkin_INCLUDE_DIRS}
${Eigen3_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
add_library( bspline_opt
src/uniform_bspline.cpp
src/bspline_optimizer.cpp
src/gradient_descent_optimizer.cpp
)
target_link_libraries( bspline_opt
${catkin_LIBRARIES}
)

213
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/bspline_opt/include/bspline_opt/bspline_optimizer.h
Unescape View File

@ -1,213 +0,0 @@
#ifndef _BSPLINE_OPTIMIZER_H_
#define _BSPLINE_OPTIMIZER_H_
#include <Eigen/Eigen>
#include <path_searching/dyn_a_star.h>
#include <bspline_opt/uniform_bspline.h>
#include <plan_env/grid_map.h>
#include <plan_env/obj_predictor.h>
#include <ros/ros.h>
#include "bspline_opt/lbfgs.hpp"
#include <traj_utils/plan_container.hpp>
// Gradient and elasitc band optimization
// Input: a signed distance field and a sequence of points
// Output: the optimized sequence of points
// The format of points: N x 3 matrix, each row is a point
namespace ego_planner
{
class ControlPoints
{
public:
double clearance;
int size;
Eigen::MatrixXd points;
std::vector<std::vector<Eigen::Vector3d>> base_point; // The point at the statrt of the direction vector (collision point)
std::vector<std::vector<Eigen::Vector3d>> direction; // Direction vector, must be normalized.
std::vector<bool> flag_temp; // A flag that used in many places. Initialize it everytime before using it.
// std::vector<bool> occupancy;
void resize(const int size_set)
{
size = size_set;
base_point.clear();
direction.clear();
flag_temp.clear();
// occupancy.clear();
points.resize(3, size_set);
base_point.resize(size);
direction.resize(size);
flag_temp.resize(size);
// occupancy.resize(size);
}
void segment(ControlPoints &buf, const int start, const int end)
{
if (start < 0 || end >= size || points.rows() != 3)
{
ROS_ERROR("Wrong segment index! start=%d, end=%d", start, end);
return;
}
buf.resize(end - start + 1);
buf.points = points.block(0, start, 3, end - start + 1);
buf.clearance = clearance;
buf.size = end - start + 1;
for (int i = start; i <= end; i++)
{
buf.base_point[i - start] = base_point[i];
buf.direction[i - start] = direction[i];
// if ( buf.base_point[i - start].size() > 1 )
// {
// ROS_ERROR("buf.base_point[i - start].size()=%d, base_point[i].size()=%d", buf.base_point[i - start].size(), base_point[i].size());
// }
}
// cout << "RichInfoOneSeg_temp, insede" << endl;
// for ( int k=0; k<buf.size; k++ )
// if ( buf.base_point[k].size() > 0 )
// {
// cout << "###" << buf.points.col(k).transpose() << endl;
// for (int k2 = 0; k2 < buf.base_point[k].size(); k2++)
// {
// cout << " " << buf.base_point[k][k2].transpose() << " @ " << buf.direction[k][k2].transpose() << endl;
// }
// }
}
};
class BsplineOptimizer
{
public:
BsplineOptimizer() {}
~BsplineOptimizer() {}
/* main API */
void setEnvironment(const GridMap::Ptr &map);
void setEnvironment(const GridMap::Ptr &map, const fast_planner::ObjPredictor::Ptr mov_obj);
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);
/* helper function */
// required inputs
void setControlPoints(const Eigen::MatrixXd &points);
void setBsplineInterval(const double &ts);
void setSwarmTrajs(SwarmTrajData *swarm_trajs_ptr);
void setDroneId(const int drone_id);
// optional inputs
void setGuidePath(const vector<Eigen::Vector3d> &guide_pt);
void setWaypoints(const vector<Eigen::Vector3d> &waypts,
const vector<int> &waypt_idx); // N-2 constraints at most
void setLocalTargetPt(const Eigen::Vector3d local_target_pt) { local_target_pt_ = local_target_pt; };
void optimize();
ControlPoints getControlPoints() { return cps_; };
AStar::Ptr a_star_;
std::vector<Eigen::Vector3d> ref_pts_;
std::vector<ControlPoints> distinctiveTrajs(vector<std::pair<int, int>> segments);
std::vector<std::pair<int, int>> initControlPoints(Eigen::MatrixXd &init_points, bool flag_first_init = true);
bool BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double ts); // must be called after initControlPoints()
bool BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double &final_cost, const ControlPoints &control_points, double ts);
bool BsplineOptimizeTrajRefine(const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points);
inline int getOrder(void) { return order_; }
inline double getSwarmClearance(void) { return swarm_clearance_; }
private:
GridMap::Ptr grid_map_;
fast_planner::ObjPredictor::Ptr moving_objs_;
SwarmTrajData *swarm_trajs_{NULL}; // Can not use shared_ptr and no need to free
int drone_id_;
enum FORCE_STOP_OPTIMIZE_TYPE
{
DONT_STOP,
STOP_FOR_REBOUND,
STOP_FOR_ERROR
} force_stop_type_;
// main input
// Eigen::MatrixXd control_points_; // B-spline control points, N x dim
double bspline_interval_; // B-spline knot span
Eigen::Vector3d end_pt_; // end of the trajectory
// int dim_; // dimension of the B-spline
//
vector<Eigen::Vector3d> guide_pts_; // geometric guiding path points, N-6
vector<Eigen::Vector3d> waypoints_; // waypts constraints
vector<int> waypt_idx_; // waypts constraints index
//
int max_num_id_, max_time_id_; // stopping criteria
int cost_function_; // used to determine objective function
double start_time_; // global time for moving obstacles
/* optimization parameters */
int order_; // bspline degree
double lambda1_; // jerk smoothness weight
double lambda2_, new_lambda2_; // distance weight
double lambda3_; // feasibility weight
double lambda4_; // curve fitting
int a;
//
double dist0_, swarm_clearance_; // safe distance
double max_vel_, max_acc_; // dynamic limits
int variable_num_; // optimization variables
int iter_num_; // iteration of the solver
Eigen::VectorXd best_variable_; //
double min_cost_; //
Eigen::Vector3d local_target_pt_;
#define INIT_min_ellip_dist_ 123456789.0123456789
double min_ellip_dist_;
ControlPoints cps_;
/* cost function */
/* calculate each part of cost function with control points q as input */
static double costFunction(const std::vector<double> &x, std::vector<double> &grad, void *func_data);
void combineCost(const std::vector<double> &x, vector<double> &grad, double &cost);
// q contains all control points
void calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient, bool falg_use_jerk = true);
void calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
void calcTerminalCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
void calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost);
void calcMovingObjCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
void calcSwarmCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
void calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
bool check_collision_and_rebound(void);
static int earlyExit(void *func_data, const double *x, const double *g, const double fx, const double xnorm, const double gnorm, const double step, int n, int k, int ls);
static double costFunctionRebound(void *func_data, const double *x, double *grad, const int n);
static double costFunctionRefine(void *func_data, const double *x, double *grad, const int n);
bool rebound_optimize(double &final_cost);
bool refine_optimize();
void combineCostRebound(const double *x, double *grad, double &f_combine, const int n);
void combineCostRefine(const double *x, double *grad, double &f_combine, const int n);
/* for benckmark evaluation only */
public:
typedef unique_ptr<BsplineOptimizer> Ptr;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace ego_planner
#endif

52
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h
Unescape View File

@ -1,52 +0,0 @@
#ifndef _GRADIENT_DESCENT_OPT_H_
#define _GRADIENT_DESCENT_OPT_H_
#include <iostream>
#include <vector>
#include <Eigen/Eigen>
using namespace std;
class GradientDescentOptimizer
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
typedef double (*objfunDef)(const Eigen::VectorXd &x, Eigen::VectorXd &grad, bool &force_return, void *data);
enum RESULT
{
FIND_MIN,
FAILED,
RETURN_BY_ORDER,
REACH_MAX_ITERATION
};
GradientDescentOptimizer(int v_num, objfunDef objf, void *f_data)
{
variable_num_ = v_num;
objfun_ = objf;
f_data_ = f_data;
};
void set_maxiter(int limit) { iter_limit_ = limit; }
void set_maxeval(int limit) { invoke_limit_ = limit; }
void set_xtol_rel(double xtol_rel) { xtol_rel_ = xtol_rel; }
void set_xtol_abs(double xtol_abs) { xtol_abs_ = xtol_abs; }
void set_min_grad(double min_grad) { min_grad_ = min_grad; }
RESULT optimize(Eigen::VectorXd &x_init_optimal, double &opt_f);
private:
int variable_num_{0};
int iter_limit_{1e10};
int invoke_limit_{1e10};
double xtol_rel_;
double xtol_abs_;
double min_grad_;
double time_limit_;
void *f_data_;
objfunDef objfun_;
};
#endif

1451
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/bspline_opt/include/bspline_opt/lbfgs.hpp
View File

File diff suppressed because it is too large Load Diff

80
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/bspline_opt/include/bspline_opt/uniform_bspline.h
Unescape View File

@ -1,80 +0,0 @@
#ifndef _UNIFORM_BSPLINE_H_
#define _UNIFORM_BSPLINE_H_
#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>
using namespace std;
namespace ego_planner
{
// An implementation of non-uniform B-spline with different dimensions
// It also represents uniform B-spline which is a special case of non-uniform
class UniformBspline
{
private:
// control points for B-spline with different dimensions.
// Each row represents one single control point
// The dimension is determined by column number
// e.g. B-spline with N points in 3D space -> Nx3 matrix
Eigen::MatrixXd control_points_;
int p_, n_, m_; // p degree, n+1 control points, m = n+p+1
Eigen::VectorXd u_; // knots vector
double interval_; // knot span \delta t
Eigen::MatrixXd getDerivativeControlPoints();
double limit_vel_, limit_acc_, limit_ratio_, feasibility_tolerance_; // physical limits and time adjustment ratio
public:
UniformBspline() {}
UniformBspline(const Eigen::MatrixXd &points, const int &order, const double &interval);
~UniformBspline();
Eigen::MatrixXd get_control_points(void) { return control_points_; }
// initialize as an uniform B-spline
void setUniformBspline(const Eigen::MatrixXd &points, const int &order, const double &interval);
// get / set basic bspline info
void setKnot(const Eigen::VectorXd &knot);
Eigen::VectorXd getKnot();
Eigen::MatrixXd getControlPoint();
double getInterval();
bool getTimeSpan(double &um, double &um_p);
// compute position / derivative
Eigen::VectorXd evaluateDeBoor(const double &u); // use u \in [up, u_mp]
inline Eigen::VectorXd evaluateDeBoorT(const double &t) { return evaluateDeBoor(t + u_(p_)); } // use t \in [0, duration]
UniformBspline getDerivative();
// 3D B-spline interpolation of points in point_set, with boundary vel&acc
// constraints
// input : (K+2) points with boundary vel/acc; ts
// output: (K+6) control_pts
static void parameterizeToBspline(const double &ts, const vector<Eigen::Vector3d> &point_set,
const vector<Eigen::Vector3d> &start_end_derivative,
Eigen::MatrixXd &ctrl_pts);
/* check feasibility, adjust time */
void setPhysicalLimits(const double &vel, const double &acc, const double &tolerance);
bool checkFeasibility(double &ratio, bool show = false);
void lengthenTime(const double &ratio);
/* for performance evaluation */
double getTimeSum();
double getLength(const double &res = 0.01);
double getJerk();
void getMeanAndMaxVel(double &mean_v, double &max_v);
void getMeanAndMaxAcc(double &mean_a, double &max_a);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace ego_planner
#endif

77
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/bspline_opt/package.xml
Unescape View File

@ -1,77 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>bspline_opt</name>
<version>0.0.0</version>
<description>The bspline_opt package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/bspline_opt</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>plan_env</build_depend>
<build_depend>path_searching</build_depend>
<build_depend>traj_utils</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>plan_env</build_export_depend>
<build_export_depend>path_searching</build_export_depend>
<build_export_depend>traj_utils</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>plan_env</exec_depend>
<exec_depend>path_searching</exec_depend>
<exec_depend>traj_utils</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

1735
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/bspline_opt/src/bspline_optimizer.cpp
View File

File diff suppressed because it is too large Load Diff

94
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/bspline_opt/src/gradient_descent_optimizer.cpp
Unescape View File

@ -1,94 +0,0 @@
#include <bspline_opt/gradient_descent_optimizer.h>
#define RESET "\033[0m"
#define RED "\033[31m"
GradientDescentOptimizer::RESULT
GradientDescentOptimizer::optimize(Eigen::VectorXd &x_init_optimal, double &opt_f)
{
if (min_grad_ < 1e-10)
{
cout << RED << "min_grad_ is invalid:" << min_grad_ << RESET << endl;
return FAILED;
}
if (iter_limit_ <= 2)
{
cout << RED << "iter_limit_ is invalid:" << iter_limit_ << RESET << endl;
return FAILED;
}
void *f_data = f_data_;
int iter = 2;
int invoke_count = 2;
bool force_return;
Eigen::VectorXd x_k(x_init_optimal), x_kp1(x_init_optimal.rows());
double cost_k, cost_kp1, cost_min;
Eigen::VectorXd grad_k(x_init_optimal.rows()), grad_kp1(x_init_optimal.rows());
cost_k = objfun_(x_k, grad_k, force_return, f_data);
if (force_return)
return RETURN_BY_ORDER;
cost_min = cost_k;
double max_grad = max(abs(grad_k.maxCoeff()), abs(grad_k.minCoeff()));
constexpr double MAX_MOVEMENT_AT_FIRST_ITERATION = 0.1; // meter
double alpha0 = max_grad < MAX_MOVEMENT_AT_FIRST_ITERATION ? 1.0 : (MAX_MOVEMENT_AT_FIRST_ITERATION / max_grad);
x_kp1 = x_k - alpha0 * grad_k;
cost_kp1 = objfun_(x_kp1, grad_kp1, force_return, f_data);
if (force_return)
return RETURN_BY_ORDER;
if (cost_min > cost_kp1)
cost_min = cost_kp1;
/*** start iteration ***/
while (++iter <= iter_limit_ && invoke_count <= invoke_limit_)
{
Eigen::VectorXd s = x_kp1 - x_k;
Eigen::VectorXd y = grad_kp1 - grad_k;
double alpha = s.dot(y) / y.dot(y);
if (isnan(alpha) || isinf(alpha))
{
cout << RED << "step size invalid! alpha=" << alpha << RESET << endl;
return FAILED;
}
if (iter % 2) // to aviod copying operations
{
do
{
x_k = x_kp1 - alpha * grad_kp1;
cost_k = objfun_(x_k, grad_k, force_return, f_data);
invoke_count++;
if (force_return)
return RETURN_BY_ORDER;
alpha *= 0.5;
} while (cost_k > cost_kp1 - 1e-4 * alpha * grad_kp1.transpose() * grad_kp1); // Armijo condition
if (grad_k.norm() < min_grad_)
{
opt_f = cost_k;
return FIND_MIN;
}
}
else
{
do
{
x_kp1 = x_k - alpha * grad_k;
cost_kp1 = objfun_(x_kp1, grad_kp1, force_return, f_data);
invoke_count++;
if (force_return)
return RETURN_BY_ORDER;
alpha *= 0.5;
} while (cost_kp1 > cost_k - 1e-4 * alpha * grad_k.transpose() * grad_k); // Armijo condition
if (grad_kp1.norm() < min_grad_)
{
opt_f = cost_kp1;
return FIND_MIN;
}
}
}
opt_f = iter_limit_ % 2 ? cost_k : cost_kp1;
return REACH_MAX_ITERATION;
}

377
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/bspline_opt/src/uniform_bspline.cpp
Unescape View File

@ -1,377 +0,0 @@
#include "bspline_opt/uniform_bspline.h"
#include <ros/ros.h>
namespace ego_planner
{
UniformBspline::UniformBspline(const Eigen::MatrixXd &points, const int &order,
const double &interval)
{
setUniformBspline(points, order, interval);
}
UniformBspline::~UniformBspline() {}
void UniformBspline::setUniformBspline(const Eigen::MatrixXd &points, const int &order,
const double &interval)
{
control_points_ = points;
p_ = order;
interval_ = interval;
n_ = points.cols() - 1;
m_ = n_ + p_ + 1;
u_ = Eigen::VectorXd::Zero(m_ + 1);
for (int i = 0; i <= m_; ++i)
{
if (i <= p_)
{
u_(i) = double(-p_ + i) * interval_;
}
else if (i > p_ && i <= m_ - p_)
{
u_(i) = u_(i - 1) + interval_;
}
else if (i > m_ - p_)
{
u_(i) = u_(i - 1) + interval_;
}
}
}
void UniformBspline::setKnot(const Eigen::VectorXd &knot) { this->u_ = knot; }
Eigen::VectorXd UniformBspline::getKnot() { return this->u_; }
bool UniformBspline::getTimeSpan(double &um, double &um_p)
{
if (p_ > u_.rows() || m_ - p_ > u_.rows())
return false;
um = u_(p_);
um_p = u_(m_ - p_);
return true;
}
Eigen::MatrixXd UniformBspline::getControlPoint() { return control_points_; }
Eigen::VectorXd UniformBspline::evaluateDeBoor(const double &u)
{
double ub = min(max(u_(p_), u), u_(m_ - p_));
// determine which [ui,ui+1] lay in
int k = p_;
while (true)
{
if (u_(k + 1) >= ub)
break;
++k;
}
/* deBoor's alg */
vector<Eigen::VectorXd> d;
for (int i = 0; i <= p_; ++i)
{
d.push_back(control_points_.col(k - p_ + i));
// cout << d[i].transpose() << endl;
}
for (int r = 1; r <= p_; ++r)
{
for (int i = p_; i >= r; --i)
{
double alpha = (ub - u_[i + k - p_]) / (u_[i + 1 + k - r] - u_[i + k - p_]);
// cout << "alpha: " << alpha << endl;
d[i] = (1 - alpha) * d[i - 1] + alpha * d[i];
}
}
return d[p_];
}
// Eigen::VectorXd UniformBspline::evaluateDeBoorT(const double& t) {
// return evaluateDeBoor(t + u_(p_));
// }
Eigen::MatrixXd UniformBspline::getDerivativeControlPoints()
{
// The derivative of a b-spline is also a b-spline, its order become p_-1
// control point Qi = p_*(Pi+1-Pi)/(ui+p_+1-ui+1)
Eigen::MatrixXd ctp(control_points_.rows(), control_points_.cols() - 1);
for (int i = 0; i < ctp.cols(); ++i)
{
ctp.col(i) =
p_ * (control_points_.col(i + 1) - control_points_.col(i)) / (u_(i + p_ + 1) - u_(i + 1));
}
return ctp;
}
UniformBspline UniformBspline::getDerivative()
{
Eigen::MatrixXd ctp = getDerivativeControlPoints();
UniformBspline derivative(ctp, p_ - 1, interval_);
/* cut the first and last knot */
Eigen::VectorXd knot(u_.rows() - 2);
knot = u_.segment(1, u_.rows() - 2);
derivative.setKnot(knot);
return derivative;
}
double UniformBspline::getInterval() { return interval_; }
void UniformBspline::setPhysicalLimits(const double &vel, const double &acc, const double &tolerance)
{
limit_vel_ = vel;
limit_acc_ = acc;
limit_ratio_ = 1.1;
feasibility_tolerance_ = tolerance;
}
bool UniformBspline::checkFeasibility(double &ratio, bool show)
{
bool fea = true;
Eigen::MatrixXd P = control_points_;
int dimension = control_points_.rows();
/* check vel feasibility and insert points */
double max_vel = -1.0;
double enlarged_vel_lim = limit_vel_ * (1.0 + feasibility_tolerance_) + 1e-4;
for (int i = 0; i < P.cols() - 1; ++i)
{
Eigen::VectorXd vel = p_ * (P.col(i + 1) - P.col(i)) / (u_(i + p_ + 1) - u_(i + 1));
if (fabs(vel(0)) > enlarged_vel_lim || fabs(vel(1)) > enlarged_vel_lim ||
fabs(vel(2)) > enlarged_vel_lim)
{
if (show)
cout << "[Check]: Infeasible vel " << i << " :" << vel.transpose() << endl;
fea = false;
for (int j = 0; j < dimension; ++j)
{
max_vel = max(max_vel, fabs(vel(j)));
}
}
}
/* acc feasibility */
double max_acc = -1.0;
double enlarged_acc_lim = limit_acc_ * (1.0 + feasibility_tolerance_) + 1e-4;
for (int i = 0; i < P.cols() - 2; ++i)
{
Eigen::VectorXd acc = p_ * (p_ - 1) *
((P.col(i + 2) - P.col(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
(P.col(i + 1) - P.col(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
(u_(i + p_ + 1) - u_(i + 2));
if (fabs(acc(0)) > enlarged_acc_lim || fabs(acc(1)) > enlarged_acc_lim ||
fabs(acc(2)) > enlarged_acc_lim)
{
if (show)
cout << "[Check]: Infeasible acc " << i << " :" << acc.transpose() << endl;
fea = false;
for (int j = 0; j < dimension; ++j)
{
max_acc = max(max_acc, fabs(acc(j)));
}
}
}
ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_));
return fea;
}
void UniformBspline::lengthenTime(const double &ratio)
{
int num1 = 5;
int num2 = getKnot().rows() - 1 - 5;
double delta_t = (ratio - 1.0) * (u_(num2) - u_(num1));
double t_inc = delta_t / double(num2 - num1);
for (int i = num1 + 1; i <= num2; ++i)
u_(i) += double(i - num1) * t_inc;
for (int i = num2 + 1; i < u_.rows(); ++i)
u_(i) += delta_t;
}
// void UniformBspline::recomputeInit() {}
void UniformBspline::parameterizeToBspline(const double &ts, const vector<Eigen::Vector3d> &point_set,
const vector<Eigen::Vector3d> &start_end_derivative,
Eigen::MatrixXd &ctrl_pts)
{
if (ts <= 0)
{
cout << "[B-spline]:time step error." << endl;
return;
}
if (point_set.size() <= 3)
{
cout << "[B-spline]:point set have only " << point_set.size() << " points." << endl;
return;
}
if (start_end_derivative.size() != 4)
{
cout << "[B-spline]:derivatives error." << endl;
}
int K = point_set.size();
// write A
Eigen::Vector3d prow(3), vrow(3), arow(3);
prow << 1, 4, 1;
vrow << -1, 0, 1;
arow << 1, -2, 1;
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 4, K + 2);
for (int i = 0; i < K; ++i)
A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose();
A.block(K, 0, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
A.block(K + 2, 0, 1, 3) = (1 / ts / ts) * arow.transpose();
A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) * arow.transpose();
//cout << "A" << endl << A << endl << endl;
// write b
Eigen::VectorXd bx(K + 4), by(K + 4), bz(K + 4);
for (int i = 0; i < K; ++i)
{
bx(i) = point_set[i](0);
by(i) = point_set[i](1);
bz(i) = point_set[i](2);
}
for (int i = 0; i < 4; ++i)
{
bx(K + i) = start_end_derivative[i](0);
by(K + i) = start_end_derivative[i](1);
bz(K + i) = start_end_derivative[i](2);
}
// solve Ax = b
Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx);
Eigen::VectorXd py = A.colPivHouseholderQr().solve(by);
Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz);
// convert to control pts
ctrl_pts.resize(3, K + 2);
ctrl_pts.row(0) = px.transpose();
ctrl_pts.row(1) = py.transpose();
ctrl_pts.row(2) = pz.transpose();
// cout << "[B-spline]: parameterization ok." << endl;
}
double UniformBspline::getTimeSum()
{
double tm, tmp;
if (getTimeSpan(tm, tmp))
return tmp - tm;
else
return -1.0;
}
double UniformBspline::getLength(const double &res)
{
double length = 0.0;
double dur = getTimeSum();
Eigen::VectorXd p_l = evaluateDeBoorT(0.0), p_n;
for (double t = res; t <= dur + 1e-4; t += res)
{
p_n = evaluateDeBoorT(t);
length += (p_n - p_l).norm();
p_l = p_n;
}
return length;
}
double UniformBspline::getJerk()
{
UniformBspline jerk_traj = getDerivative().getDerivative().getDerivative();
Eigen::VectorXd times = jerk_traj.getKnot();
Eigen::MatrixXd ctrl_pts = jerk_traj.getControlPoint();
int dimension = ctrl_pts.rows();
double jerk = 0.0;
for (int i = 0; i < ctrl_pts.cols(); ++i)
{
for (int j = 0; j < dimension; ++j)
{
jerk += (times(i + 1) - times(i)) * ctrl_pts(j, i) * ctrl_pts(j, i);
}
}
return jerk;
}
void UniformBspline::getMeanAndMaxVel(double &mean_v, double &max_v)
{
UniformBspline vel = getDerivative();
double tm, tmp;
vel.getTimeSpan(tm, tmp);
double max_vel = -1.0, mean_vel = 0.0;
int num = 0;
for (double t = tm; t <= tmp; t += 0.01)
{
Eigen::VectorXd vxd = vel.evaluateDeBoor(t);
double vn = vxd.norm();
mean_vel += vn;
++num;
if (vn > max_vel)
{
max_vel = vn;
}
}
mean_vel = mean_vel / double(num);
mean_v = mean_vel;
max_v = max_vel;
}
void UniformBspline::getMeanAndMaxAcc(double &mean_a, double &max_a)
{
UniformBspline acc = getDerivative().getDerivative();
double tm, tmp;
acc.getTimeSpan(tm, tmp);
double max_acc = -1.0, mean_acc = 0.0;
int num = 0;
for (double t = tm; t <= tmp; t += 0.01)
{
Eigen::VectorXd axd = acc.evaluateDeBoor(t);
double an = axd.norm();
mean_acc += an;
++num;
if (an > max_acc)
{
max_acc = an;
}
}
mean_acc = mean_acc / double(num);
mean_a = mean_acc;
max_a = max_acc;
}
} // namespace ego_planner

130
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/CMakeLists.txt
Unescape View File

@ -1,130 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(drone_detect)
set(CMAKE_BUILD_TYPE "Release")
ADD_COMPILE_OPTIONS(-std=c++11 )
ADD_COMPILE_OPTIONS(-std=c++14 )
## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
## enforcing cleaner code.
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
## Find catkin macros and libraries
find_package(catkin REQUIRED
COMPONENTS
roscpp
sensor_msgs
roslint
cv_bridge
message_filters
)
## Find system libraries
find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Boost REQUIRED)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS
include
## This is only necessary because Eigen3 sets a non-standard EIGEN3_INCLUDE_DIR variable
${EIGEN3_INCLUDE_DIR}
LIBRARIES
CATKIN_DEPENDS
roscpp
sensor_msgs
DEPENDS OpenCV Eigen Boost
## find_package(Eigen3) provides a non standard EIGEN3_INCLUDE_DIR instead of Eigen3_INCLUDE_DIRS.
## Therefore, the DEPEND does not work as expected and we need to add the directory to the INCLUDE_DIRS
# Eigen3
## Boost is not part of the DEPENDS since it is only used in source files,
## Dependees do not depend on Boost when they depend on this package.
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
# Set manually because Eigen sets a non standard INCLUDE DIR
${EIGEN3_INCLUDE_DIR}
# Set because Boost is an internal dependency, not transitive.
${Boost_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
## Declare cpp executables
add_executable(${PROJECT_NAME}
src/${PROJECT_NAME}_node.cpp
src/drone_detector.cpp
)
target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_11)
## Add dependencies to exported targets, like ROS msgs or srvs
add_dependencies(${PROJECT_NAME}
${catkin_EXPORTED_TARGETS}
)
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
${OpenCV_LIBS}
)
#############
## Install ##
#############
# Mark executables and/or libraries for installation
install(
TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# Mark cpp header files for installation
install(
DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
# Mark other files for installation
install(
DIRECTORY doc
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
# if(${CATKIN_ENABLE_TESTING})
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
# ## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test
# test/test_drone_detector.cpp)
# target_link_libraries(${PROJECT_NAME}-test)
# endif()
##########################
## Static code analysis ##
##########################
roslint_cpp()

29
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/LICENSE
Unescape View File

@ -1,29 +0,0 @@
Software License Agreement (BSD License)
Copyright (c) 2017, Peter Fankhauser
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of Autonomous Systems Lab nor ETH Zurich nor the names
of its contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

97
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/README.md
Unescape View File

@ -1,97 +0,0 @@
# Drone Detect
## Overview
This is a package for detecting other drones in depth image and then calculating their true pose error in the world frame of the observer drone.
![Example image](doc/demo.jpg)
## Usage
You can launch the node alongside the main after you assigning the right topic name
```
roslaunch drone_detect drone_detect.launch
```
or add the following code in `run_in_sim.launch`
```xml
<node pkg="drone_detect" type="drone_detect" name="drone_$(arg drone_id)_drone_detect" output="screen">
<rosparam command="load" file="$(find local_sensing_node)/params/camera.yaml" />
<rosparam command="load" file="$(find drone_detect)/config/default.yaml"/>
<param name="my_id" value="$(arg drone_id)" />
<remap from="~odometry" to="/drone_$(arg drone_id)_$(arg odom_topic)"/>
<remap from="~depth" to="/drone_$(arg drone_id)_pcl_render_node/depth"/>
<remap from="~colordepth" to="/drone_$(arg drone_id)_pcl_render_node/colordepth"/>
<remap from="~camera_pose" to="/drone_$(arg drone_id)_pcl_render_node/camera_pose"/>
<remap from="~drone0" to="/drone_0_$(arg odom_topic)"/>
<remap from="~drone1" to="/drone_1_$(arg odom_topic)"/>
<remap from="~drone2" to="/drone_2_$(arg odom_topic)"/>
</node>
```
## Config files
* **camera.yaml** : The camera intrinsics are stored in this file
* **default.yaml**: Some parameters related to drone detection.
```yaml
debug_flag: true
# the proportion of the pixel threshold to the total pixels projected from the drone to the depth map
estimate/pixel_ratio: 0.1
# the radius of the pose errror sphere
estimate/max_pose_error: 0.4
# the width and height of the model of drone
estimate/drone_width: 0.5
estimate/drone_height: 0.1
```
## Nodes
#### Subscribed Topics
* **`/depth`** ([sensor_msgs::Image])
The depth image from pcl_render_node.
* **`/colordepth`**([sensor_msgs::Image])
The color image from pcl_render_node
* **`/camera_pose`** ([geometry_msgs::PoseStamped])
The camere pose from pcl_render_node
The above three topics are synchronized when in use, the callback function is **`rcvDepthColorCamPoseCallback`**
- **`/dronex_odom_sub_`** ([nav_msgs::Odometry])
The odometry of other drones
#### Published Topics
* **`/new_depth`** ([sensor_msgs::Image])
The new depth image after erasing the moving drones
* **`/new_colordepth`**([sensor_msgs::Image])
The color image with some debug marks
* **`/camera_pose_error`** ([geometry_msgs::PoseStamped])
The pose error of detected drones in world frame of the observer drone.

7
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/config/camera.yaml
Unescape View File

@ -1,7 +0,0 @@
cam_width: 640
cam_height: 480
cam_fx: 386.02557373046875
cam_fy: 386.02557373046875
cam_cx: 321.8554382324219
cam_cy: 241.2396240234375

5
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/config/default.yaml
Unescape View File

@ -1,5 +0,0 @@
# debug_flag: true
pixel_ratio: 0.1
estimate/max_pose_error: 0.4
estimate/drone_width: 0.5
estimate/drone_height: 0.2

BIN
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/doc/demo.jpg
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 364 KiB

BIN
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/doc/example.jpg
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 103 KiB

156
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/include/drone_detector/drone_detector.h
Unescape View File

@ -1,156 +0,0 @@
#pragma once
#include <iostream>
#include <vector>
// ROS
#include <ros/ros.h>
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <cv_bridge/cv_bridge.h>
// synchronize topic
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <std_srvs/Trigger.h>
//include opencv and eigen
#include <Eigen/Eigen>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <cv_bridge/cv_bridge.h>
namespace detect {
const int max_drone_num_ = 3;
/*!
* Main class for the node to handle the ROS interfacing.
*/
class DroneDetector
{
public:
/*!
* Constructor.
* @param nodeHandle the ROS node handle.
*/
DroneDetector(ros::NodeHandle& nodeHandle);
/*!
* Destructor.
*/
virtual ~DroneDetector();
void test();
private:
void readParameters();
// inline functions
double getDist2(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2);
double getDist2(const Eigen::Vector4d &p1, const Eigen::Vector4d &p2);
Eigen::Vector4d depth2Pos(int u, int v, float depth);
Eigen::Vector4d depth2Pos(const Eigen::Vector2i &pixel, float depth) ;
Eigen::Vector2i pos2Depth(const Eigen::Vector4d &pose_in_camera) ;
bool isInSensorRange(const Eigen::Vector2i &pixel);
bool countPixel(int drone_id, Eigen::Vector2i &true_pixel, Eigen::Vector4d &true_pose_cam);
void detect(int drone_id, Eigen::Vector2i &true_pixel);
// subscribe callback function
void rcvDepthColorCamPoseCallback(const sensor_msgs::ImageConstPtr& depth_img, \
const sensor_msgs::ImageConstPtr& color_img, \
const geometry_msgs::PoseStampedConstPtr& camera_pose);
void rcvDepthCamPoseCallback(const sensor_msgs::ImageConstPtr& depth_img, \
const geometry_msgs::PoseStampedConstPtr& camera_pose);
void rcvMyOdomCallback(const nav_msgs::Odometry& odom);
void rcvDepthImgCallback(const sensor_msgs::ImageConstPtr& depth_img);
void rcvDroneOdomCallbackBase(const nav_msgs::Odometry& odom, const int drone_id);
void rcvDrone0OdomCallback(const nav_msgs::Odometry& odom);
void rcvDrone1OdomCallback(const nav_msgs::Odometry& odom);
void rcvDrone2OdomCallback(const nav_msgs::Odometry& odom);
void rcvDroneXOdomCallback(const nav_msgs::Odometry& odom);
//! ROS node handle.
ros::NodeHandle& nh_;
//! ROS topic subscriber.
// depth, colordepth, camera_pos subscriber
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, geometry_msgs::PoseStamped> SyncPolicyDepthColorImagePose;
typedef std::shared_ptr<message_filters::Synchronizer<SyncPolicyDepthColorImagePose>> SynchronizerDepthColorImagePose;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::PoseStamped> SyncPolicyDepthImagePose;
typedef std::shared_ptr<message_filters::Synchronizer<SyncPolicyDepthImagePose>> SynchronizerDepthImagePose;
// std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> depth_img_sub_;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> colordepth_img_sub_;
std::shared_ptr<message_filters::Subscriber<geometry_msgs::PoseStamped>> camera_pos_sub_;
SynchronizerDepthColorImagePose sync_depth_color_img_pose_;
SynchronizerDepthImagePose sync_depth_img_pose_;
// other drones subscriber
ros::Subscriber drone0_odom_sub_, drone1_odom_sub_, drone2_odom_sub_, droneX_odom_sub_;
std::string drone1_odom_topic_, drone2_odom_topic_;
ros::Subscriber my_odom_sub_, depth_img_sub_;
bool has_odom_;
nav_msgs::Odometry my_odom_;
// ROS topic publisher
// new_depth_img: erase the detected drones
// new_colordepth_img: for debug
ros::Publisher new_depth_img_pub_;
ros::Publisher debug_depth_img_pub_;
// parameters
//camera param
int img_width_, img_height_;
double fx_,fy_,cx_,cy_;
double max_pose_error_;
double max_pose_error2_;
double drone_width_, drone_height_;
double pixel_ratio_;
int pixel_threshold_;
// for debug
bool debug_flag_;
int debug_detect_result_[max_drone_num_];
std::stringstream debug_img_text_[max_drone_num_];
ros::Time debug_start_time_, debug_end_time_;
ros::Publisher debug_info_pub_;
ros::Publisher drone_pose_err_pub_[max_drone_num_];
int my_id_;
cv::Mat depth_img_, color_img_;
Eigen::Matrix4d cam2body_;
Eigen::Matrix4d cam2world_;
Eigen::Quaterniond cam2world_quat_;
Eigen::Vector4d my_pose_world_;
Eigen::Quaterniond my_attitude_world_;
Eigen::Vector4d my_last_pose_world_;
ros::Time my_last_odom_stamp_ = ros::TIME_MAX;
ros::Time my_last_camera_stamp_ = ros::TIME_MAX;
Eigen::Matrix4d drone2world_[max_drone_num_];
Eigen::Vector4d drone_pose_world_[max_drone_num_];
Eigen::Quaterniond drone_attitude_world_[max_drone_num_];
Eigen::Vector4d drone_pose_cam_[max_drone_num_];
Eigen::Vector2i drone_ref_pixel_[max_drone_num_];
std::vector<Eigen::Vector2i> hit_pixels_[max_drone_num_];
int valid_pixel_cnt_[max_drone_num_];
bool in_depth_[max_drone_num_] = {false};
cv::Point searchbox_lu_[max_drone_num_], searchbox_rd_[max_drone_num_];
cv::Point boundingbox_lu_[max_drone_num_], boundingbox_rd_[max_drone_num_];
};
} /* namespace */

24
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/launch/drone_detect.launch
Unescape View File

@ -1,24 +0,0 @@
<launch>
<arg name="my_id" value="1"/>
<arg name="odom_topic" value="/vins_estimator/imu_propagate"/>
<!-- Launch ROS Package Template Node -->
<node pkg="drone_detect" type="drone_detect" name="test_drone_detect" output="screen">
<rosparam command="load" file="$(find drone_detect)/config/camera.yaml" />
<rosparam command="load" file="$(find drone_detect)/config/default.yaml"/>
<param name="my_id" value="$(arg my_id)" />
<param name="debug_flag" value="false" />
<remap from="~odometry" to="$(arg odom_topic)"/>
<remap from="~depth" to="/camera/depth/image_rect_raw"/>
<!-- <remap from="~camera_pose" to="/drone_$(arg my_id)_pcl_render_node/camera_pose"/> -->
<!-- <remap from="~drone0" to="/drone_0_$(arg odom_topic)"/>
<remap from="~drone1" to="/drone_1_$(arg odom_topic)"/>
<remap from="~drone2" to="/drone_2_$(arg odom_topic)"/> -->
<!-- <remap from="~drone1" to="/test/dynamic0_odom"/> -->
<!-- <remap from="~drone2" to="/test/dynamic1_odom"/> -->
</node>
</launch>

27
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/launch/ros_package_template_overlying_params.launch
Unescape View File

@ -1,27 +0,0 @@
<launch>
<!-- This launch file shows how to launch ROS nodes with default parameters and some user's
custom parameters:
* Default parameters are loaded form the file specified by 'default_param_file';
* User's overlying parameters file path is specified by 'overlying_param_file', which can
be set from the launch file that includes this file.
The user can overwrite even just a subset of the default parameters. Only parameters
contained in 'overlying_param_file' will overwrite the corresponding default ones.
This means that if the user does not specify some parameters, then the default ones
will be loaded. -->
<!-- Default parameters. Note the usage of 'value', to avoid they can be wrongly changed. -->
<arg name="default_param_file" value="$(dirname)/../config/default.yaml" />
<!-- User's parameters that can overly default ones: use default ones in case user does not specify them. -->
<arg name="overlying_param_file" default="$(arg default_param_file)" />
<!-- Launch ROS Package Template node. -->
<node pkg="ros_package_template" type="ros_package_template" name="ros_package_template" output="screen">
<rosparam command="load" file="$(arg default_param_file)" />
<!-- Overlay parameters if user specified them. They must be loaded after default parameters! -->
<rosparam command="load" file="$(arg overlying_param_file)" />
</node>
</launch>

20
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/package.xml
Unescape View File

@ -1,20 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>drone_detect</name>
<version>0.1.0</version>
<description>Detect other drones in depth image.</description>
<maintainer email="zjujiangchaozhu@gmail.com">Jiangchao Zhu</maintainer>
<license>BSD</license>
<author email="zjujiangchaozhu@gmail.com">Jiangchao Zhu</author>
<!-- buildtool_depend: dependencies of the build process -->
<buildtool_depend>catkin</buildtool_depend>
<!-- build_depend: dependencies only used in source files -->
<build_depend>boost</build_depend>
<!-- depend: build, export, and execution dependency -->
<depend>eigen</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<build_depend>roslint</build_depend>
</package>

14
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/src/drone_detect_node.cpp
Unescape View File

@ -1,14 +0,0 @@
#include <ros/ros.h>
#include "drone_detector/drone_detector.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "drone_detect");
ros::NodeHandle nh("~");
detect::DroneDetector drone_detector(nh);
drone_detector.test();
ros::spin();
return 0;
}

423
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/src/drone_detector.cpp
Unescape View File

@ -1,423 +0,0 @@
#include "drone_detector/drone_detector.h"
// STD
#include <string>
namespace detect {
DroneDetector::DroneDetector(ros::NodeHandle& nodeHandle)
: nh_(nodeHandle)
{
readParameters();
// depth_img_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(nh_, "depth", 50, ros::TransportHints().tcpNoDelay()));
// colordepth_img_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(nh_, "colordepth", 50));
// camera_pos_sub_.reset(new message_filters::Subscriber<geometry_msgs::PoseStamped>(nh_, "camera_pose", 50));
my_odom_sub_ = nh_.subscribe("odometry", 100, &DroneDetector::rcvMyOdomCallback, this, ros::TransportHints().tcpNoDelay());
depth_img_sub_ = nh_.subscribe("depth", 50, &DroneDetector::rcvDepthImgCallback, this, ros::TransportHints().tcpNoDelay());
// sync_depth_color_img_pose_->registerCallback(boost::bind(&DroneDetector::rcvDepthColorCamPoseCallback, this, _1, _2, _3));
// drone0_odom_sub_ = nh_.subscribe("drone0", 50, &DroneDetector::rcvDrone0OdomCallback, this);
// drone1_odom_sub_ = nh_.subscribe("drone1", 50, &DroneDetector::rcvDrone1OdomCallback, this);
// drone2_odom_sub_ = nh_.subscribe("drone2", 50, &DroneDetector::rcvDrone2OdomCallback, this);
droneX_odom_sub_ = nh_.subscribe("/others_odom", 100, &DroneDetector::rcvDroneXOdomCallback, this, ros::TransportHints().tcpNoDelay());
new_depth_img_pub_ = nh_.advertise<sensor_msgs::Image>("new_depth_image", 50);
debug_depth_img_pub_ = nh_.advertise<sensor_msgs::Image>("debug_depth_image", 50);
debug_info_pub_ = nh_.advertise<std_msgs::String>("/debug_info", 50);
cam2body_ << 0.0, 0.0, 1.0, 0.0,
-1.0, 0.0, 0.0, 0.0,
0.0, -1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0;
// init drone_pose_err_pub
for(int i = 0; i < max_drone_num_; i++) {
if(i != my_id_)
drone_pose_err_pub_[i] = nh_.advertise<geometry_msgs::PoseStamped>("drone"+std::to_string(i)+"to"+std::to_string(my_id_)+"_pose_err", 50);
}
ROS_INFO("Successfully launched node.");
}
DroneDetector::~DroneDetector()
{
}
void DroneDetector::readParameters()
{
// camera params
nh_.getParam("cam_width", img_width_);
nh_.getParam("cam_height", img_height_);
nh_.getParam("cam_fx", fx_);
nh_.getParam("cam_fy", fy_);
nh_.getParam("cam_cx", cx_);
nh_.getParam("cam_cy", cy_);
//
nh_.getParam("debug_flag", debug_flag_);
nh_.getParam("pixel_ratio", pixel_ratio_);
nh_.getParam("my_id", my_id_);
nh_.getParam("estimate/drone_width", drone_width_);
nh_.getParam("estimate/drone_height", drone_height_);
nh_.getParam("estimate/max_pose_error", max_pose_error_);
max_pose_error2_ = max_pose_error_*max_pose_error_;
}
// inline functions
inline double DroneDetector::getDist2(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2)
{
double delta_x = p1(0)-p2(0);
double delta_y = p1(1)-p2(1);
double delta_z = p1(2)-p2(2);
return delta_x*delta_x+delta_y*delta_y+delta_z*delta_z;
}
inline double DroneDetector::getDist2(const Eigen::Vector4d &p1, const Eigen::Vector4d &p2)
{
double delta_x = p1(0)-p2(0);
double delta_y = p1(1)-p2(1);
double delta_z = p1(2)-p2(2);
return delta_x*delta_x+delta_y*delta_y+delta_z*delta_z;
}
inline Eigen::Vector4d DroneDetector::depth2Pos(int u, int v, float depth)
{
Eigen::Vector4d pose_in_camera;
pose_in_camera(0) = (u - cx_) * depth / fx_;
pose_in_camera(1) = (v - cy_) * depth / fy_;
pose_in_camera(2) = depth;
pose_in_camera(3) = 1.0;
return pose_in_camera;
}
inline Eigen::Vector4d DroneDetector::depth2Pos(const Eigen::Vector2i &pixel, float depth)
{
Eigen::Vector4d pose_in_camera;
pose_in_camera(0) = (pixel(0) - cx_) * depth / fx_;
pose_in_camera(1) = (pixel(1) - cy_) * depth / fy_;
pose_in_camera(2) = depth;
pose_in_camera(3) = 1.0;
return pose_in_camera;
}
inline Eigen::Vector2i DroneDetector::pos2Depth(const Eigen::Vector4d &pose_in_camera)
{
float depth = pose_in_camera(2);
Eigen::Vector2i pixel;
pixel(0) = pose_in_camera(0) * fx_ / depth + cx_ + 0.5;
pixel(1) = pose_in_camera(1) * fy_ / depth + cy_ + 0.5;
return pixel;
}
inline bool DroneDetector::isInSensorRange(const Eigen::Vector2i &pixel)
{
if (pixel(0)>=0 && pixel(1) >= 0 && pixel(0) <= img_width_ && pixel(1) <= img_height_) return true;
else
return false;
}
void DroneDetector::rcvMyOdomCallback(const nav_msgs::Odometry& odom)
{
my_odom_ = odom;
Eigen::Matrix4d body2world = Eigen::Matrix4d::Identity();
my_pose_world_(0) = odom.pose.pose.position.x;
my_pose_world_(1) = odom.pose.pose.position.y;
my_pose_world_(2) = odom.pose.pose.position.z;
my_pose_world_(3) = 1.0;
my_attitude_world_.x() = odom.pose.pose.orientation.x;
my_attitude_world_.y() = odom.pose.pose.orientation.y;
my_attitude_world_.z() = odom.pose.pose.orientation.z;
my_attitude_world_.w() = odom.pose.pose.orientation.w;
body2world.block<3,3>(0,0) = my_attitude_world_.toRotationMatrix();
body2world(0,3) = my_pose_world_(0);
body2world(1,3) = my_pose_world_(1);
body2world(2,3) = my_pose_world_(2);
//convert to cam pose
cam2world_ = body2world * cam2body_;
cam2world_quat_ = cam2world_.block<3,3>(0,0);
// my_last_odom_stamp_ = odom.header.stamp;
// my_last_pose_world_(0) = odom.pose.pose.position.x;
// my_last_pose_world_(1) = odom.pose.pose.position.y;
// my_last_pose_world_(2) = odom.pose.pose.position.z;
// my_last_pose_world_(3) = 1.0;
//publish tf
// static tf::TransformBroadcaster br;
// tf::Transform transform;
// transform.setOrigin( tf::Vector3(cam2world(0,3), cam2world(1,3), cam2world(2,3) ));
// transform.setRotation(tf::Quaternion(cam2world_quat.x(), cam2world_quat.y(), cam2world_quat.z(), cam2world_quat.w()));
// br.sendTransform(tf::StampedTransform(transform, my_last_odom_stamp, "world", "camera"));
//publish transform from world frame to quadrotor frame.
}
void DroneDetector::rcvDepthImgCallback(const sensor_msgs::ImageConstPtr& depth_img)
{
/* get depth image */
cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(depth_img, depth_img->encoding);
cv_ptr->image.copyTo(depth_img_);
debug_start_time_ = ros::Time::now();
Eigen::Vector2i true_pixel[max_drone_num_];
for (int i = 0; i < max_drone_num_; i++) {
if (in_depth_[i]) {
detect(i, true_pixel[i]);
}
}
cv_bridge::CvImage out_msg;
for (int i = 0; i < max_drone_num_; i++) {
if (in_depth_[i]) {
// erase hit pixels in depth
for(int k = 0; k < int(hit_pixels_[i].size()); k++) {
// depth_img_.at<float>(hit_pixels_[i][k](1), hit_pixels_[i][k](0)) = 0;
uint16_t *row_ptr;
row_ptr = depth_img_.ptr<uint16_t>(hit_pixels_[i][k](1));
(*(row_ptr+hit_pixels_[i][k](0))) = 0.0;
}
}
}
debug_end_time_ = ros::Time::now();
// ROS_WARN("cost_total_time = %lf", (debug_end_time_ - debug_start_time_).toSec()*1000.0);
out_msg.header = depth_img->header;
out_msg.encoding = depth_img->encoding;
out_msg.image = depth_img_.clone();
new_depth_img_pub_.publish(out_msg.toImageMsg());
std_msgs::String msg;
std::stringstream ss;
if(debug_flag_) {
for (int i = 0; i < max_drone_num_; i++) {
if (in_depth_[i]) {
// add bound box in colormap
// cv::Rect rect(_bbox_lu.x, _bbox_lu.y, _bbox_rd.x, _bbox_rd.y);//左上坐标（x,y）和矩形的长(x)宽(y)
cv::rectangle(depth_img_, cv::Rect(searchbox_lu_[i], searchbox_rd_[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0);
cv::rectangle(depth_img_, cv::Rect(boundingbox_lu_[i], boundingbox_rd_[i]), cv::Scalar(0, 0, 0), 5, cv::LINE_8, 0);
if (debug_detect_result_[i] == 1) {
ss << "no enough " << hit_pixels_[i].size();
} else if(debug_detect_result_[i] == 2) {
ss << "success";
}
} else {
ss << "no detect";
}
}
out_msg.header = depth_img->header;
out_msg.encoding = depth_img->encoding;
out_msg.image = depth_img_.clone();
debug_depth_img_pub_.publish(out_msg.toImageMsg());
msg.data = ss.str();
debug_info_pub_.publish(msg);
}
}
void DroneDetector::rcvDroneOdomCallbackBase(const nav_msgs::Odometry& odom, int drone_id)
{
if (drone_id == my_id_) {
return;
}
Eigen::Matrix4d drone2world = Eigen::Matrix4d::Identity();
drone_pose_world_[drone_id](0) = odom.pose.pose.position.x;
drone_pose_world_[drone_id](1) = odom.pose.pose.position.y;
drone_pose_world_[drone_id](2) = odom.pose.pose.position.z;
drone_pose_world_[drone_id](3) = 1.0;
drone_attitude_world_[drone_id].x() = odom.pose.pose.orientation.x;
drone_attitude_world_[drone_id].y() = odom.pose.pose.orientation.y;
drone_attitude_world_[drone_id].z() = odom.pose.pose.orientation.z;
drone_attitude_world_[drone_id].w() = odom.pose.pose.orientation.w;
drone2world.block<3,3>(0,0) = drone_attitude_world_[drone_id].toRotationMatrix();
drone2world(0,3) = drone_pose_world_[drone_id](0);
drone2world(1,3) = drone_pose_world_[drone_id](1);
drone2world(2,3) = drone_pose_world_[drone_id](2);
drone_pose_cam_[drone_id] = cam2world_.inverse() * drone_pose_world_[drone_id];
// if the drone is in sensor range
drone_ref_pixel_[drone_id] = pos2Depth(drone_pose_cam_[drone_id]);
if (drone_pose_cam_[drone_id](2) > 0 && isInSensorRange(drone_ref_pixel_[drone_id])) {
in_depth_[drone_id] = true;
} else {
in_depth_[drone_id] = false;
debug_detect_result_[drone_id] = 0;
}
}
void DroneDetector::rcvDrone0OdomCallback(const nav_msgs::Odometry& odom)
{
rcvDroneOdomCallbackBase(odom, 0);
}
void DroneDetector::rcvDrone1OdomCallback(const nav_msgs::Odometry& odom)
{
rcvDroneOdomCallbackBase(odom, 1);
}
void DroneDetector::rcvDrone2OdomCallback(const nav_msgs::Odometry& odom)
{
rcvDroneOdomCallbackBase(odom, 2);
}
void DroneDetector::rcvDroneXOdomCallback(const nav_msgs::Odometry& odom)
{
std::string numstr = odom.child_frame_id.substr(6);
try
{
int drone_id = std::stoi(numstr);
rcvDroneOdomCallbackBase(odom, drone_id);
}
catch(const std::exception& e)
{
std::cout << e.what() << '\n';
}
}
bool DroneDetector::countPixel(int drone_id, Eigen::Vector2i &true_pixel, Eigen::Vector4d &true_pose_cam)
{
boundingbox_lu_[drone_id].x = img_width_;
boundingbox_rd_[drone_id].x = 0;
boundingbox_lu_[drone_id].y = img_height_;
boundingbox_rd_[drone_id].y = 0;
valid_pixel_cnt_[drone_id] = 0;
hit_pixels_[drone_id].clear();
Eigen::Vector2i tmp_pixel;
Eigen::Vector4d tmp_pose_cam;
int search_radius = 2*max_pose_error_*fx_/drone_pose_cam_[drone_id](2);
float depth;
searchbox_lu_[drone_id].x = drone_ref_pixel_[drone_id](0) - search_radius;
searchbox_lu_[drone_id].y = drone_ref_pixel_[drone_id](1) - search_radius;
searchbox_rd_[drone_id].x = drone_ref_pixel_[drone_id](0) + search_radius;
searchbox_rd_[drone_id].y = drone_ref_pixel_[drone_id](1) + search_radius;
// check the tmp_p around ref_pixel
for(int i = -search_radius; i <= search_radius; i++)
for(int j = -search_radius; j <= search_radius; j++)
{
tmp_pixel(0) = drone_ref_pixel_[drone_id](0) + j;
tmp_pixel(1) = drone_ref_pixel_[drone_id](1) + i;
if(tmp_pixel(0) < 0 || tmp_pixel(0) >= img_width_ || tmp_pixel(1) < 0 || tmp_pixel(1) >= img_height_)
continue;
// depth = depth_img_.at<float>(tmp_pixel(1), tmp_pixel(0));
uint16_t *row_ptr;
row_ptr = depth_img_.ptr<uint16_t>(tmp_pixel(1));
depth = (*(row_ptr+tmp_pixel(0))) / 1000.0;
// ROS_WARN("depth = %lf", depth);
// get tmp_pose in cam frame
tmp_pose_cam = depth2Pos(tmp_pixel(0), tmp_pixel(1), depth);
double dist2 = getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]);
// ROS_WARN("dist2 = %lf", dist2);
if (dist2 < max_pose_error2_) {
valid_pixel_cnt_[drone_id]++;
hit_pixels_[drone_id].push_back(tmp_pixel);
boundingbox_lu_[drone_id].x = tmp_pixel(0) < boundingbox_lu_[drone_id].x ? tmp_pixel(0) : boundingbox_lu_[drone_id].x;
boundingbox_lu_[drone_id].y = tmp_pixel(1) < boundingbox_lu_[drone_id].y ? tmp_pixel(1) : boundingbox_lu_[drone_id].y;
boundingbox_rd_[drone_id].x = tmp_pixel(0) > boundingbox_rd_[drone_id].x ? tmp_pixel(0) : boundingbox_rd_[drone_id].x;
boundingbox_rd_[drone_id].y = tmp_pixel(1) > boundingbox_rd_[drone_id].y ? tmp_pixel(1) : boundingbox_rd_[drone_id].y;
}
}
pixel_threshold_ = (drone_width_*fx_/drone_pose_cam_[drone_id](2)) * (drone_height_*fy_/drone_pose_cam_[drone_id](2))*pixel_ratio_;
if (valid_pixel_cnt_[drone_id] > pixel_threshold_) {
int step = 1, size = (boundingbox_rd_[drone_id].y-boundingbox_lu_[drone_id].y) < (boundingbox_rd_[drone_id].x-boundingbox_lu_[drone_id].x) ? (boundingbox_rd_[drone_id].y-boundingbox_lu_[drone_id].y) : (boundingbox_rd_[drone_id].x-boundingbox_lu_[drone_id].x);
int init_x = (boundingbox_lu_[drone_id].x+boundingbox_rd_[drone_id].x)/2, init_y = (boundingbox_lu_[drone_id].y+boundingbox_rd_[drone_id].y)/2;
int x_flag = 1, y_flag = 1;
int x_idx = 0, y_idx = 0;
uint16_t *row_ptr;
row_ptr = depth_img_.ptr<uint16_t>(tmp_pixel(1));
depth = (*(row_ptr+tmp_pixel(0))) / 1000.0;
tmp_pose_cam = depth2Pos(init_x, init_y, depth);
if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){
true_pixel(0) = init_x;
true_pixel(1) = init_y;
true_pose_cam = tmp_pose_cam;
return true;
}
while(step<size) {
while(x_idx<step){
init_x = init_x+x_flag;
uint16_t *row_ptr;
row_ptr = depth_img_.ptr<uint16_t>(tmp_pixel(1));
depth = (*(row_ptr+tmp_pixel(0))) / 1000.0;
tmp_pose_cam = depth2Pos(init_x, init_y, depth);
if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_) {
true_pixel(0) = init_x;
true_pixel(1) = init_y;
true_pose_cam = tmp_pose_cam;
return true;
}
x_idx++;
}
x_idx = 0;
x_flag = -x_flag;
while(y_idx<step){
init_y = init_y+y_flag;
uint16_t *row_ptr;
row_ptr = depth_img_.ptr<uint16_t>(tmp_pixel(1));
depth = (*(row_ptr+tmp_pixel(0))) / 1000.0;
tmp_pose_cam = depth2Pos(init_x, init_y, depth);
if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){
true_pixel(0) = init_x;
true_pixel(1) = init_y;
true_pose_cam = tmp_pose_cam;
return true;
}
y_idx++;
}
y_idx = 0;
y_flag = -y_flag;
step++;
}
while(x_idx<step-1){
init_x = init_x+x_flag;
uint16_t *row_ptr;
row_ptr = depth_img_.ptr<uint16_t>(tmp_pixel(1));
depth = (*(row_ptr+tmp_pixel(0))) / 1000.0;
tmp_pose_cam = depth2Pos(init_x, init_y, depth);
if (getDist2(tmp_pose_cam, drone_pose_cam_[drone_id]) < max_pose_error2_){
true_pixel(0) = init_x;
true_pixel(1) = init_y;
true_pose_cam = tmp_pose_cam;
return true;
}
x_idx++;
}
}
return false;
}
void DroneDetector::detect(int drone_id, Eigen::Vector2i &true_pixel)
{
Eigen::Vector4d true_pose_cam, pose_error;
bool found = countPixel(drone_id, true_pixel, true_pose_cam);
if (found) {
// ROS_WARN("FOUND");
pose_error = cam2world_*true_pose_cam - drone_pose_world_[drone_id];
debug_detect_result_[drone_id] = 2;
geometry_msgs::PoseStamped out_msg;
out_msg.header.stamp = my_last_camera_stamp_;
out_msg.header.frame_id = "/drone_detect";
out_msg.pose.position.x = pose_error(0);
out_msg.pose.position.y = pose_error(1);
out_msg.pose.position.z = pose_error(2);
drone_pose_err_pub_[drone_id].publish(out_msg);
} else {
// ROS_WARN("NOT FOUND");
debug_detect_result_[drone_id] = 1;
}
}
void DroneDetector::test() {
ROS_WARN("my_id = %d", my_id_);
}
} /* namespace */

10
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/drone_detect/test/test_drone_detector.cpp
Unescape View File

@ -1,10 +0,0 @@
// gtest
#include <gtest/gtest.h>
// Run all the tests that were declared with TEST()
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
srand((int)time(0));
return RUN_ALL_TESTS();
}

42
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/path_searching/CMakeLists.txt
Unescape View File

@ -1,42 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(path_searching)
set(CMAKE_BUILD_TYPE "Release")
ADD_COMPILE_OPTIONS(-std=c++11 )
ADD_COMPILE_OPTIONS(-std=c++14 )
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
find_package(Eigen3 REQUIRED)
find_package(PCL 1.7 REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
visualization_msgs
plan_env
cv_bridge
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES path_searching
CATKIN_DEPENDS plan_env
# DEPENDS system_lib
)
include_directories(
SYSTEM
include
${catkin_INCLUDE_DIRS}
${Eigen3_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
add_library( path_searching
src/dyn_a_star.cpp
)
target_link_libraries( path_searching
${catkin_LIBRARIES}
)

115
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/path_searching/include/path_searching/dyn_a_star.h
Unescape View File

@ -1,115 +0,0 @@
#ifndef _DYN_A_STAR_H_
#define _DYN_A_STAR_H_
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include <plan_env/grid_map.h>
#include <queue>
constexpr double inf = 1 >> 20;
struct GridNode;
typedef GridNode *GridNodePtr;
struct GridNode
{
enum enum_state
{
OPENSET = 1,
CLOSEDSET = 2,
UNDEFINED = 3
};
int rounds{0}; // Distinguish every call
enum enum_state state
{
UNDEFINED
};
Eigen::Vector3i index;
double gScore{inf}, fScore{inf};
GridNodePtr cameFrom{NULL};
};
class NodeComparator
{
public:
bool operator()(GridNodePtr node1, GridNodePtr node2)
{
return node1->fScore > node2->fScore;
}
};
class AStar
{
private:
GridMap::Ptr grid_map_;
inline void coord2gridIndexFast(const double x, const double y, const double z, int &id_x, int &id_y, int &id_z);
double getDiagHeu(GridNodePtr node1, GridNodePtr node2);
double getManhHeu(GridNodePtr node1, GridNodePtr node2);
double getEuclHeu(GridNodePtr node1, GridNodePtr node2);
inline double getHeu(GridNodePtr node1, GridNodePtr node2);
bool ConvertToIndexAndAdjustStartEndPoints(const Eigen::Vector3d start_pt, const Eigen::Vector3d end_pt, Eigen::Vector3i &start_idx, Eigen::Vector3i &end_idx);
inline Eigen::Vector3d Index2Coord(const Eigen::Vector3i &index) const;
inline bool Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const;
//bool (*checkOccupancyPtr)( const Eigen::Vector3d &pos );
inline bool checkOccupancy(const Eigen::Vector3d &pos) { return (bool)grid_map_->getInflateOccupancy(pos); }
std::vector<GridNodePtr> retrievePath(GridNodePtr current);
double step_size_, inv_step_size_;
Eigen::Vector3d center_;
Eigen::Vector3i CENTER_IDX_, POOL_SIZE_;
const double tie_breaker_ = 1.0 + 1.0 / 10000;
std::vector<GridNodePtr> gridPath_;
GridNodePtr ***GridNodeMap_;
std::priority_queue<GridNodePtr, std::vector<GridNodePtr>, NodeComparator> openSet_;
int rounds_{0};
public:
typedef std::shared_ptr<AStar> Ptr;
AStar(){};
~AStar();
void initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size);
bool AstarSearch(const double step_size, Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
std::vector<Eigen::Vector3d> getPath();
};
inline double AStar::getHeu(GridNodePtr node1, GridNodePtr node2)
{
return tie_breaker_ * getDiagHeu(node1, node2);
}
inline Eigen::Vector3d AStar::Index2Coord(const Eigen::Vector3i &index) const
{
return ((index - CENTER_IDX_).cast<double>() * step_size_) + center_;
};
inline bool AStar::Coord2Index(const Eigen::Vector3d &pt, Eigen::Vector3i &idx) const
{
idx = ((pt - center_) * inv_step_size_ + Eigen::Vector3d(0.5, 0.5, 0.5)).cast<int>() + CENTER_IDX_;
if (idx(0) < 0 || idx(0) >= POOL_SIZE_(0) || idx(1) < 0 || idx(1) >= POOL_SIZE_(1) || idx(2) < 0 || idx(2) >= POOL_SIZE_(2))
{
ROS_ERROR("Ran out of pool, index=%d %d %d", idx(0), idx(1), idx(2));
return false;
}
return true;
};
#endif

71
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/path_searching/package.xml
Unescape View File

@ -1,71 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>path_searching</name>
<version>0.0.0</version>
<description>The path_searching package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/path_searching</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>plan_env</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>plan_env</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>plan_env</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

260
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/path_searching/src/dyn_a_star.cpp
Unescape View File

@ -1,260 +0,0 @@
#include "path_searching/dyn_a_star.h"
using namespace std;
using namespace Eigen;
AStar::~AStar()
{
for (int i = 0; i < POOL_SIZE_(0); i++)
for (int j = 0; j < POOL_SIZE_(1); j++)
for (int k = 0; k < POOL_SIZE_(2); k++)
delete GridNodeMap_[i][j][k];
}
void AStar::initGridMap(GridMap::Ptr occ_map, const Eigen::Vector3i pool_size)
{
POOL_SIZE_ = pool_size;
CENTER_IDX_ = pool_size / 2;
GridNodeMap_ = new GridNodePtr **[POOL_SIZE_(0)];
for (int i = 0; i < POOL_SIZE_(0); i++)
{
GridNodeMap_[i] = new GridNodePtr *[POOL_SIZE_(1)];
for (int j = 0; j < POOL_SIZE_(1); j++)
{
GridNodeMap_[i][j] = new GridNodePtr[POOL_SIZE_(2)];
for (int k = 0; k < POOL_SIZE_(2); k++)
{
GridNodeMap_[i][j][k] = new GridNode;
}
}
}
grid_map_ = occ_map;
}
double AStar::getDiagHeu(GridNodePtr node1, GridNodePtr node2)
{
double dx = abs(node1->index(0) - node2->index(0));
double dy = abs(node1->index(1) - node2->index(1));
double dz = abs(node1->index(2) - node2->index(2));
double h = 0.0;
int diag = min(min(dx, dy), dz);
dx -= diag;
dy -= diag;
dz -= diag;
if (dx == 0)
{
h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dy, dz) + 1.0 * abs(dy - dz);
}
if (dy == 0)
{
h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dz) + 1.0 * abs(dx - dz);
}
if (dz == 0)
{
h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dy) + 1.0 * abs(dx - dy);
}
return h;
}
double AStar::getManhHeu(GridNodePtr node1, GridNodePtr node2)
{
double dx = abs(node1->index(0) - node2->index(0));
double dy = abs(node1->index(1) - node2->index(1));
double dz = abs(node1->index(2) - node2->index(2));
return dx + dy + dz;
}
double AStar::getEuclHeu(GridNodePtr node1, GridNodePtr node2)
{
return (node2->index - node1->index).norm();
}
vector<GridNodePtr> AStar::retrievePath(GridNodePtr current)
{
vector<GridNodePtr> path;
path.push_back(current);
while (current->cameFrom != NULL)
{
current = current->cameFrom;
path.push_back(current);
}
return path;
}
bool AStar::ConvertToIndexAndAdjustStartEndPoints(Vector3d start_pt, Vector3d end_pt, Vector3i &start_idx, Vector3i &end_idx)
{
if (!Coord2Index(start_pt, start_idx) || !Coord2Index(end_pt, end_idx))
return false;
if (checkOccupancy(Index2Coord(start_idx)))
{
//ROS_WARN("Start point is insdide an obstacle.");
do
{
start_pt = (start_pt - end_pt).normalized() * step_size_ + start_pt;
if (!Coord2Index(start_pt, start_idx))
return false;
} while (checkOccupancy(Index2Coord(start_idx)));
}
if (checkOccupancy(Index2Coord(end_idx)))
{
//ROS_WARN("End point is insdide an obstacle.");
do
{
end_pt = (end_pt - start_pt).normalized() * step_size_ + end_pt;
if (!Coord2Index(end_pt, end_idx))
return false;
} while (checkOccupancy(Index2Coord(end_idx)));
}
return true;
}
bool AStar::AstarSearch(const double step_size, Vector3d start_pt, Vector3d end_pt)
{
ros::Time time_1 = ros::Time::now();
++rounds_;
step_size_ = step_size;
inv_step_size_ = 1 / step_size;
center_ = (start_pt + end_pt) / 2;
Vector3i start_idx, end_idx;
if (!ConvertToIndexAndAdjustStartEndPoints(start_pt, end_pt, start_idx, end_idx))
{
ROS_ERROR("Unable to handle the initial or end point, force return!");
return false;
}
// if ( start_pt(0) > -1 && start_pt(0) < 0 )
// cout << "start_pt=" << start_pt.transpose() << " end_pt=" << end_pt.transpose() << endl;
GridNodePtr startPtr = GridNodeMap_[start_idx(0)][start_idx(1)][start_idx(2)];
GridNodePtr endPtr = GridNodeMap_[end_idx(0)][end_idx(1)][end_idx(2)];
std::priority_queue<GridNodePtr, std::vector<GridNodePtr>, NodeComparator> empty;
openSet_.swap(empty);
GridNodePtr neighborPtr = NULL;
GridNodePtr current = NULL;
startPtr->index = start_idx;
startPtr->rounds = rounds_;
startPtr->gScore = 0;
startPtr->fScore = getHeu(startPtr, endPtr);
startPtr->state = GridNode::OPENSET; //put start node in open set
startPtr->cameFrom = NULL;
openSet_.push(startPtr); //put start in open set
endPtr->index = end_idx;
double tentative_gScore;
int num_iter = 0;
while (!openSet_.empty())
{
num_iter++;
current = openSet_.top();
openSet_.pop();
// if ( num_iter < 10000 )
// cout << "current=" << current->index.transpose() << endl;
if (current->index(0) == endPtr->index(0) && current->index(1) == endPtr->index(1) && current->index(2) == endPtr->index(2))
{
// ros::Time time_2 = ros::Time::now();
// printf("\033[34mA star iter:%d, time:%.3f\033[0m\n",num_iter, (time_2 - time_1).toSec()*1000);
// if((time_2 - time_1).toSec() > 0.1)
// ROS_WARN("Time consume in A star path finding is %f", (time_2 - time_1).toSec() );
gridPath_ = retrievePath(current);
return true;
}
current->state = GridNode::CLOSEDSET; //move current node from open set to closed set.
for (int dx = -1; dx <= 1; dx++)
for (int dy = -1; dy <= 1; dy++)
for (int dz = -1; dz <= 1; dz++)
{
if (dx == 0 && dy == 0 && dz == 0)
continue;
Vector3i neighborIdx;
neighborIdx(0) = (current->index)(0) + dx;
neighborIdx(1) = (current->index)(1) + dy;
neighborIdx(2) = (current->index)(2) + dz;
if (neighborIdx(0) < 1 || neighborIdx(0) >= POOL_SIZE_(0) - 1 || neighborIdx(1) < 1 || neighborIdx(1) >= POOL_SIZE_(1) - 1 || neighborIdx(2) < 1 || neighborIdx(2) >= POOL_SIZE_(2) - 1)
{
continue;
}
neighborPtr = GridNodeMap_[neighborIdx(0)][neighborIdx(1)][neighborIdx(2)];
neighborPtr->index = neighborIdx;
bool flag_explored = neighborPtr->rounds == rounds_;
if (flag_explored && neighborPtr->state == GridNode::CLOSEDSET)
{
continue; //in closed set.
}
neighborPtr->rounds = rounds_;
if (checkOccupancy(Index2Coord(neighborPtr->index)))
{
continue;
}
double static_cost = sqrt(dx * dx + dy * dy + dz * dz);
tentative_gScore = current->gScore + static_cost;
if (!flag_explored)
{
//discover a new node
neighborPtr->state = GridNode::OPENSET;
neighborPtr->cameFrom = current;
neighborPtr->gScore = tentative_gScore;
neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr);
openSet_.push(neighborPtr); //put neighbor in open set and record it.
}
else if (tentative_gScore < neighborPtr->gScore)
{ //in open set and need update
neighborPtr->cameFrom = current;
neighborPtr->gScore = tentative_gScore;
neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr);
}
}
ros::Time time_2 = ros::Time::now();
if ((time_2 - time_1).toSec() > 0.2)
{
ROS_WARN("Failed in A star path searching !!! 0.2 seconds time limit exceeded.");
return false;
}
}
ros::Time time_2 = ros::Time::now();
if ((time_2 - time_1).toSec() > 0.1)
ROS_WARN("Time consume in A star path finding is %.3fs, iter=%d", (time_2 - time_1).toSec(), num_iter);
return false;
}
vector<Vector3d> AStar::getPath()
{
vector<Vector3d> path;
for (auto ptr : gridPath_)
path.push_back(Index2Coord(ptr->index));
reverse(path.begin(), path.end());
return path;
}

59
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_env/CMakeLists.txt
Unescape View File

@ -1,59 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(plan_env)
set(CMAKE_BUILD_TYPE "Release")
ADD_COMPILE_OPTIONS(-std=c++11 )
ADD_COMPILE_OPTIONS(-std=c++14 )
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
find_package(OpenCV REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
visualization_msgs
cv_bridge
message_filters
)
find_package(Eigen3 REQUIRED)
find_package(PCL 1.7 REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES plan_env
CATKIN_DEPENDS roscpp std_msgs
DEPENDS OpenCV
# DEPENDS system_lib
)
include_directories(
SYSTEM
include
${catkin_INCLUDE_DIRS}
${Eigen3_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
link_directories(${PCL_LIBRARY_DIRS})
add_library( plan_env
src/grid_map.cpp
src/raycast.cpp
src/obj_predictor.cpp
)
target_link_libraries( plan_env
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${OpenCV_LIBS}
)
add_executable(obj_generator
src/obj_generator.cpp
)
target_link_libraries(obj_generator
${catkin_LIBRARIES}
)

804
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_env/include/plan_env/grid_map.h
Unescape View File

@ -1,804 +0,0 @@
#ifndef _GRID_MAP_H
#define _GRID_MAP_H
#include <Eigen/Eigen>
#include <Eigen/StdVector>
#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
#include <random>
#include <nav_msgs/Odometry.h>
#include <queue>
#include <ros/ros.h>
#include <tuple>
#include <visualization_msgs/Marker.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/time_synchronizer.h>
#include <plan_env/raycast.h>
#define logit(x) (log((x) / (1 - (x))))
using namespace std;
// voxel hashing
template <typename T>
struct matrix_hash : std::unary_function<T, size_t> {
std::size_t operator()(T const& matrix) const {
size_t seed = 0;
for (size_t i = 0; i < matrix.size(); ++i) {
auto elem = *(matrix.data() + i);
seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
}
return seed;
}
};
// constant parameters
struct MappingParameters {
/* map properties */
Eigen::Vector3d map_origin_, map_size_;
Eigen::Vector3d map_min_boundary_, map_max_boundary_; // map range in pos
Eigen::Vector3i map_voxel_num_; // map range in index
Eigen::Vector3d local_update_range_;
double resolution_, resolution_inv_;
double obstacles_inflation_;
string frame_id_;
int pose_type_;
/* camera parameters */
double cx_, cy_, fx_, fy_;
/* time out */
double odom_depth_timeout_;
/* depth image projection filtering */
double depth_filter_maxdist_, depth_filter_mindist_, depth_filter_tolerance_;
int depth_filter_margin_;
bool use_depth_filter_;
double k_depth_scaling_factor_;
int skip_pixel_;
/* raycasting */
double p_hit_, p_miss_, p_min_, p_max_, p_occ_; // occupancy probability
double prob_hit_log_, prob_miss_log_, clamp_min_log_, clamp_max_log_,
min_occupancy_log_; // logit of occupancy probability
double min_ray_length_, max_ray_length_; // range of doing raycasting
/* local map update and clear */
int local_map_margin_;
/* visualization and computation time display */
double visualization_truncate_height_, virtual_ceil_height_, ground_height_, virtual_ceil_yp_, virtual_ceil_yn_;
bool show_occ_time_;
/* active mapping */
double unknown_flag_;
};
// intermediate mapping data for fusion
struct MappingData {
// main map data, occupancy of each voxel and Euclidean distance
std::vector<double> occupancy_buffer_;
std::vector<char> occupancy_buffer_inflate_;
// camera position and pose data
Eigen::Vector3d camera_pos_, last_camera_pos_;
Eigen::Matrix3d camera_r_m_, last_camera_r_m_;
Eigen::Matrix4d cam2body_;
// depth image data
cv::Mat depth_image_, last_depth_image_;
int image_cnt_;
// flags of map state
bool occ_need_update_, local_updated_;
bool has_first_depth_;
bool has_odom_, has_cloud_;
// odom_depth_timeout_
ros::Time last_occ_update_time_;
bool flag_depth_odom_timeout_;
bool flag_use_depth_fusion;
// depth image projected point cloud
vector<Eigen::Vector3d> proj_points_;
int proj_points_cnt;
// flag buffers for speeding up raycasting
vector<short> count_hit_, count_hit_and_miss_;
vector<char> flag_traverse_, flag_rayend_;
char raycast_num_;
queue<Eigen::Vector3i> cache_voxel_;
// range of updating grid
Eigen::Vector3i local_bound_min_, local_bound_max_;
// computation time
double fuse_time_, max_fuse_time_;
int update_num_;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class GridMap {
public:
GridMap() {}
~GridMap() {}
enum { POSE_STAMPED = 1, ODOMETRY = 2, INVALID_IDX = -10000 };
// occupancy map management
void resetBuffer();
void resetBuffer(Eigen::Vector3d min, Eigen::Vector3d max);
inline void posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id);
inline void indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos);
inline int toAddress(const Eigen::Vector3i& id);
inline int toAddress(int& x, int& y, int& z);
inline bool isInMap(const Eigen::Vector3d& pos);
inline bool isInMap(const Eigen::Vector3i& idx);
inline void setOccupancy(Eigen::Vector3d pos, double occ = 1);
inline void setOccupied(Eigen::Vector3d pos);
inline int getOccupancy(Eigen::Vector3d pos);
inline int getOccupancy(Eigen::Vector3i id);
inline int getInflateOccupancy(Eigen::Vector3d pos);
inline void boundIndex(Eigen::Vector3i& id);
inline bool isUnknown(const Eigen::Vector3i& id);
inline bool isUnknown(const Eigen::Vector3d& pos);
inline bool isKnownFree(const Eigen::Vector3i& id);
inline bool isKnownOccupied(const Eigen::Vector3i& id);
void initMap(ros::NodeHandle& nh);
void publishMap();
void publishMapInflate(bool all_info = false);
void publishDepth();
bool hasDepthObservation();
bool odomValid();
void getRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size);
inline double getResolution();
Eigen::Vector3d getOrigin();
int getVoxelNum();
bool getOdomDepthTimeout() { return md_.flag_depth_odom_timeout_; }
typedef std::shared_ptr<GridMap> Ptr;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
MappingParameters mp_;
MappingData md_;
// get depth image and camera pose
void depthPoseCallback(const sensor_msgs::ImageConstPtr& img,
const geometry_msgs::PoseStampedConstPtr& pose);
void extrinsicCallback(const nav_msgs::OdometryConstPtr& odom);
void depthOdomCallback(const sensor_msgs::ImageConstPtr& img, const nav_msgs::OdometryConstPtr& odom);
void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& img);
void odomCallback(const nav_msgs::OdometryConstPtr& odom);
// update occupancy by raycasting
void updateOccupancyCallback(const ros::TimerEvent& /*event*/);
void visCallback(const ros::TimerEvent& /*event*/);
// main update process
void projectDepthImage();
void raycastProcess();
void clearAndInflateLocalMap();
inline void inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts);
int setCacheOccupancy(Eigen::Vector3d pos, int occ);
Eigen::Vector3d closetPointInMap(const Eigen::Vector3d& pt, const Eigen::Vector3d& camera_pt);
// typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image,
// nav_msgs::Odometry> SyncPolicyImageOdom; typedef
// message_filters::sync_policies::ExactTime<sensor_msgs::Image,
// geometry_msgs::PoseStamped> SyncPolicyImagePose;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, nav_msgs::Odometry>
SyncPolicyImageOdom;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::PoseStamped>
SyncPolicyImagePose;
typedef shared_ptr<message_filters::Synchronizer<SyncPolicyImagePose>> SynchronizerImagePose;
typedef shared_ptr<message_filters::Synchronizer<SyncPolicyImageOdom>> SynchronizerImageOdom;
ros::NodeHandle node_;
shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> depth_sub_;
shared_ptr<message_filters::Subscriber<geometry_msgs::PoseStamped>> pose_sub_;
shared_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub_;
SynchronizerImagePose sync_image_pose_;
SynchronizerImageOdom sync_image_odom_;
ros::Subscriber indep_cloud_sub_, indep_odom_sub_, extrinsic_sub_;
ros::Publisher map_pub_, map_inf_pub_;
ros::Timer occ_timer_, vis_timer_;
//
uniform_real_distribution<double> rand_noise_;
normal_distribution<double> rand_noise2_;
default_random_engine eng_;
};
/* ============================== definition of inline function
* ============================== */
inline int GridMap::toAddress(const Eigen::Vector3i& id) {
return id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + id(1) * mp_.map_voxel_num_(2) + id(2);
}
inline int GridMap::toAddress(int& x, int& y, int& z) {
return x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z;
}
inline void GridMap::boundIndex(Eigen::Vector3i& id) {
Eigen::Vector3i id1;
id1(0) = max(min(id(0), mp_.map_voxel_num_(0) - 1), 0);
id1(1) = max(min(id(1), mp_.map_voxel_num_(1) - 1), 0);
id1(2) = max(min(id(2), mp_.map_voxel_num_(2) - 1), 0);
id = id1;
}
inline bool GridMap::isUnknown(const Eigen::Vector3i& id) {
Eigen::Vector3i id1 = id;
boundIndex(id1);
return md_.occupancy_buffer_[toAddress(id1)] < mp_.clamp_min_log_ - 1e-3;
}
inline bool GridMap::isUnknown(const Eigen::Vector3d& pos) {
Eigen::Vector3i idc;
posToIndex(pos, idc);
return isUnknown(idc);
}
inline bool GridMap::isKnownFree(const Eigen::Vector3i& id) {
Eigen::Vector3i id1 = id;
boundIndex(id1);
int adr = toAddress(id1);
// return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ &&
// md_.occupancy_buffer_[adr] < mp_.min_occupancy_log_;
return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && md_.occupancy_buffer_inflate_[adr] == 0;
}
inline bool GridMap::isKnownOccupied(const Eigen::Vector3i& id) {
Eigen::Vector3i id1 = id;
boundIndex(id1);
int adr = toAddress(id1);
return md_.occupancy_buffer_inflate_[adr] == 1;
}
inline void GridMap::setOccupied(Eigen::Vector3d pos) {
if (!isInMap(pos)) return;
Eigen::Vector3i id;
posToIndex(pos, id);
md_.occupancy_buffer_inflate_[id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) +
id(1) * mp_.map_voxel_num_(2) + id(2)] = 1;
}
inline void GridMap::setOccupancy(Eigen::Vector3d pos, double occ) {
if (occ != 1 && occ != 0) {
cout << "occ value error!" << endl;
return;
}
if (!isInMap(pos)) return;
Eigen::Vector3i id;
posToIndex(pos, id);
md_.occupancy_buffer_[toAddress(id)] = occ;
}
inline int GridMap::getOccupancy(Eigen::Vector3d pos) {
if (!isInMap(pos)) return -1;
Eigen::Vector3i id;
posToIndex(pos, id);
return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0;
}
inline int GridMap::getInflateOccupancy(Eigen::Vector3d pos) {
if (!isInMap(pos)) return -1;
Eigen::Vector3i id;
posToIndex(pos, id);
return int(md_.occupancy_buffer_inflate_[toAddress(id)]);
}
inline int GridMap::getOccupancy(Eigen::Vector3i id) {
if (id(0) < 0 || id(0) >= mp_.map_voxel_num_(0) || id(1) < 0 || id(1) >= mp_.map_voxel_num_(1) ||
id(2) < 0 || id(2) >= mp_.map_voxel_num_(2))
return -1;
return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0;
}
inline bool GridMap::isInMap(const Eigen::Vector3d& pos) {
if (pos(0) < mp_.map_min_boundary_(0) + 1e-4 || pos(1) < mp_.map_min_boundary_(1) + 1e-4 ||
pos(2) < mp_.map_min_boundary_(2) + 1e-4) {
// cout << "less than min range!" << endl;
return false;
}
if (pos(0) > mp_.map_max_boundary_(0) - 1e-4 || pos(1) > mp_.map_max_boundary_(1) - 1e-4 ||
pos(2) > mp_.map_max_boundary_(2) - 1e-4) {
return false;
}
return true;
}
inline bool GridMap::isInMap(const Eigen::Vector3i& idx) {
if (idx(0) < 0 || idx(1) < 0 || idx(2) < 0) {
return false;
}
if (idx(0) > mp_.map_voxel_num_(0) - 1 || idx(1) > mp_.map_voxel_num_(1) - 1 ||
idx(2) > mp_.map_voxel_num_(2) - 1) {
return false;
}
return true;
}
inline void GridMap::posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id) {
for (int i = 0; i < 3; ++i) id(i) = floor((pos(i) - mp_.map_origin_(i)) * mp_.resolution_inv_);
}
inline void GridMap::indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos) {
for (int i = 0; i < 3; ++i) pos(i) = (id(i) + 0.5) * mp_.resolution_ + mp_.map_origin_(i);
}
inline void GridMap::inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts) {
int num = 0;
/* ---------- + shape inflate ---------- */
// for (int x = -step; x <= step; ++x)
// {
// if (x == 0)
// continue;
// pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1), pt(2));
// }
// for (int y = -step; y <= step; ++y)
// {
// if (y == 0)
// continue;
// pts[num++] = Eigen::Vector3i(pt(0), pt(1) + y, pt(2));
// }
// for (int z = -1; z <= 1; ++z)
// {
// pts[num++] = Eigen::Vector3i(pt(0), pt(1), pt(2) + z);
// }
/* ---------- all inflate ---------- */
for (int x = -step; x <= step; ++x)
for (int y = -step; y <= step; ++y)
for (int z = -step; z <= step; ++z) {
pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z);
}
}
inline double GridMap::getResolution() { return mp_.resolution_; }
#endif
// #ifndef _GRID_MAP_H
// #define _GRID_MAP_H
// #include <Eigen/Eigen>
// #include <Eigen/StdVector>
// #include <cv_bridge/cv_bridge.h>
// #include <geometry_msgs/PoseStamped.h>
// #include <iostream>
// #include <random>
// #include <nav_msgs/Odometry.h>
// #include <queue>
// #include <ros/ros.h>
// #include <tuple>
// #include <visualization_msgs/Marker.h>
// #include <pcl/point_cloud.h>
// #include <pcl/point_types.h>
// #include <pcl_conversions/pcl_conversions.h>
// #include <message_filters/subscriber.h>
// #include <message_filters/sync_policies/approximate_time.h>
// #include <message_filters/sync_policies/exact_time.h>
// #include <message_filters/time_synchronizer.h>
// #include <plan_env/raycast.h>
// #define logit(x) (log((x) / (1 - (x))))
// using namespace std;
// // voxel hashing
// template <typename T>
// struct matrix_hash : std::unary_function<T, size_t> {
// std::size_t operator()(T const& matrix) const {
// size_t seed = 0;
// for (size_t i = 0; i < matrix.size(); ++i) {
// auto elem = *(matrix.data() + i);
// seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
// }
// return seed;
// }
// };
// // constant parameters
// struct MappingParameters {
// /* map properties */
// Eigen::Vector3d map_origin_, map_size_;
// Eigen::Vector3d map_min_boundary_, map_max_boundary_; // map range in pos
// Eigen::Vector3i map_voxel_num_; // map range in index
// Eigen::Vector3d local_update_range_;
// double resolution_, resolution_inv_;
// double obstacles_inflation_;
// string frame_id_;
// int pose_type_;
// /* camera parameters */
// double cx_, cy_, fx_, fy_;
// /* depth image projection filtering */
// double depth_filter_maxdist_, depth_filter_mindist_, depth_filter_tolerance_;
// int depth_filter_margin_;
// bool use_depth_filter_;
// double k_depth_scaling_factor_;
// int skip_pixel_;
// /* raycasting */
// double p_hit_, p_miss_, p_min_, p_max_, p_occ_; // occupancy probability
// double prob_hit_log_, prob_miss_log_, clamp_min_log_, clamp_max_log_,
// min_occupancy_log_; // logit of occupancy probability
// double min_ray_length_, max_ray_length_; // range of doing raycasting
// /* local map update and clear */
// int local_map_margin_;
// /* visualization and computation time display */
// double visualization_truncate_height_, virtual_ceil_height_, ground_height_;
// bool show_occ_time_;
// /* active mapping */
// double unknown_flag_;
// };
// // intermediate mapping data for fusion
// struct MappingData {
// // main map data, occupancy of each voxel and Euclidean distance
// std::vector<double> occupancy_buffer_;
// std::vector<char> occupancy_buffer_inflate_;
// // camera position and pose data
// Eigen::Vector3d camera_pos_, last_camera_pos_;
// Eigen::Quaterniond camera_q_, last_camera_q_;
// // depth image data
// cv::Mat depth_image_, last_depth_image_;
// int image_cnt_;
// // flags of map state
// bool occ_need_update_, local_updated_;
// bool has_first_depth_;
// bool has_odom_, has_cloud_;
// // depth image projected point cloud
// vector<Eigen::Vector3d> proj_points_;
// int proj_points_cnt;
// // flag buffers for speeding up raycasting
// vector<short> count_hit_, count_hit_and_miss_;
// vector<char> flag_traverse_, flag_rayend_;
// char raycast_num_;
// queue<Eigen::Vector3i> cache_voxel_;
// // range of updating grid
// Eigen::Vector3i local_bound_min_, local_bound_max_;
// // computation time
// double fuse_time_, max_fuse_time_;
// int update_num_;
// EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// };
// class GridMap {
// public:
// GridMap() {}
// ~GridMap() {}
// enum { POSE_STAMPED = 1, ODOMETRY = 2, INVALID_IDX = -10000 };
// // occupancy map management
// void resetBuffer();
// void resetBuffer(Eigen::Vector3d min, Eigen::Vector3d max);
// inline void posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id);
// inline void indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos);
// inline int toAddress(const Eigen::Vector3i& id);
// inline int toAddress(int& x, int& y, int& z);
// inline bool isInMap(const Eigen::Vector3d& pos);
// inline bool isInMap(const Eigen::Vector3i& idx);
// inline void setOccupancy(Eigen::Vector3d pos, double occ = 1);
// inline void setOccupied(Eigen::Vector3d pos);
// inline int getOccupancy(Eigen::Vector3d pos);
// inline int getOccupancy(Eigen::Vector3i id);
// inline int getInflateOccupancy(Eigen::Vector3d pos);
// inline void boundIndex(Eigen::Vector3i& id);
// inline bool isUnknown(const Eigen::Vector3i& id);
// inline bool isUnknown(const Eigen::Vector3d& pos);
// inline bool isKnownFree(const Eigen::Vector3i& id);
// inline bool isKnownOccupied(const Eigen::Vector3i& id);
// void initMap(ros::NodeHandle& nh);
// void publishMap();
// void publishMapInflate(bool all_info = false);
// void publishUnknown();
// void publishDepth();
// bool hasDepthObservation();
// bool odomValid();
// void getRegion(Eigen::Vector3d& ori, Eigen::Vector3d& size);
// inline double getResolution();
// Eigen::Vector3d getOrigin();
// int getVoxelNum();
// typedef std::shared_ptr<GridMap> Ptr;
// EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// private:
// MappingParameters mp_;
// MappingData md_;
// // get depth image and camera pose
// void depthPoseCallback(const sensor_msgs::ImageConstPtr& img,
// const geometry_msgs::PoseStampedConstPtr& pose);
// void depthOdomCallback(const sensor_msgs::ImageConstPtr& img, const nav_msgs::OdometryConstPtr& odom);
// void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& img);
// void odomCallback(const nav_msgs::OdometryConstPtr& odom);
// // update occupancy by raycasting
// void updateOccupancyCallback(const ros::TimerEvent& /*event*/);
// void visCallback(const ros::TimerEvent& /*event*/);
// // main update process
// void projectDepthImage();
// void raycastProcess();
// void clearAndInflateLocalMap();
// inline void inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts);
// int setCacheOccupancy(Eigen::Vector3d pos, int occ);
// Eigen::Vector3d closetPointInMap(const Eigen::Vector3d& pt, const Eigen::Vector3d& camera_pt);
// // typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image,
// // nav_msgs::Odometry> SyncPolicyImageOdom; typedef
// // message_filters::sync_policies::ExactTime<sensor_msgs::Image,
// // geometry_msgs::PoseStamped> SyncPolicyImagePose;
// typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, nav_msgs::Odometry>
// SyncPolicyImageOdom;
// typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::PoseStamped>
// SyncPolicyImagePose;
// typedef shared_ptr<message_filters::Synchronizer<SyncPolicyImagePose>> SynchronizerImagePose;
// typedef shared_ptr<message_filters::Synchronizer<SyncPolicyImageOdom>> SynchronizerImageOdom;
// ros::NodeHandle node_;
// shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> depth_sub_;
// shared_ptr<message_filters::Subscriber<geometry_msgs::PoseStamped>> pose_sub_;
// shared_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub_;
// SynchronizerImagePose sync_image_pose_;
// SynchronizerImageOdom sync_image_odom_;
// ros::Subscriber indep_cloud_sub_, indep_odom_sub_;
// ros::Publisher map_pub_, map_inf_pub_;
// ros::Publisher unknown_pub_;
// ros::Timer occ_timer_, vis_timer_;
// //
// uniform_real_distribution<double> rand_noise_;
// normal_distribution<double> rand_noise2_;
// default_random_engine eng_;
// };
// /* ============================== definition of inline function
// * ============================== */
// inline int GridMap::toAddress(const Eigen::Vector3i& id) {
// return id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + id(1) * mp_.map_voxel_num_(2) + id(2);
// }
// inline int GridMap::toAddress(int& x, int& y, int& z) {
// return x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z;
// }
// inline void GridMap::boundIndex(Eigen::Vector3i& id) {
// Eigen::Vector3i id1;
// id1(0) = max(min(id(0), mp_.map_voxel_num_(0) - 1), 0);
// id1(1) = max(min(id(1), mp_.map_voxel_num_(1) - 1), 0);
// id1(2) = max(min(id(2), mp_.map_voxel_num_(2) - 1), 0);
// id = id1;
// }
// inline bool GridMap::isUnknown(const Eigen::Vector3i& id) {
// Eigen::Vector3i id1 = id;
// boundIndex(id1);
// return md_.occupancy_buffer_[toAddress(id1)] < mp_.clamp_min_log_ - 1e-3;
// }
// inline bool GridMap::isUnknown(const Eigen::Vector3d& pos) {
// Eigen::Vector3i idc;
// posToIndex(pos, idc);
// return isUnknown(idc);
// }
// inline bool GridMap::isKnownFree(const Eigen::Vector3i& id) {
// Eigen::Vector3i id1 = id;
// boundIndex(id1);
// int adr = toAddress(id1);
// // return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ &&
// // md_.occupancy_buffer_[adr] < mp_.min_occupancy_log_;
// return md_.occupancy_buffer_[adr] >= mp_.clamp_min_log_ && md_.occupancy_buffer_inflate_[adr] == 0;
// }
// inline bool GridMap::isKnownOccupied(const Eigen::Vector3i& id) {
// Eigen::Vector3i id1 = id;
// boundIndex(id1);
// int adr = toAddress(id1);
// return md_.occupancy_buffer_inflate_[adr] == 1;
// }
// inline void GridMap::setOccupied(Eigen::Vector3d pos) {
// if (!isInMap(pos)) return;
// Eigen::Vector3i id;
// posToIndex(pos, id);
// md_.occupancy_buffer_inflate_[id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) +
// id(1) * mp_.map_voxel_num_(2) + id(2)] = 1;
// }
// inline void GridMap::setOccupancy(Eigen::Vector3d pos, double occ) {
// if (occ != 1 && occ != 0) {
// cout << "occ value error!" << endl;
// return;
// }
// if (!isInMap(pos)) return;
// Eigen::Vector3i id;
// posToIndex(pos, id);
// md_.occupancy_buffer_[toAddress(id)] = occ;
// }
// inline int GridMap::getOccupancy(Eigen::Vector3d pos) {
// if (!isInMap(pos)) return -1;
// Eigen::Vector3i id;
// posToIndex(pos, id);
// return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0;
// }
// inline int GridMap::getInflateOccupancy(Eigen::Vector3d pos) {
// if (!isInMap(pos)) return -1;
// Eigen::Vector3i id;
// posToIndex(pos, id);
// return int(md_.occupancy_buffer_inflate_[toAddress(id)]);
// }
// inline int GridMap::getOccupancy(Eigen::Vector3i id) {
// if (id(0) < 0 || id(0) >= mp_.map_voxel_num_(0) || id(1) < 0 || id(1) >= mp_.map_voxel_num_(1) ||
// id(2) < 0 || id(2) >= mp_.map_voxel_num_(2))
// return -1;
// return md_.occupancy_buffer_[toAddress(id)] > mp_.min_occupancy_log_ ? 1 : 0;
// }
// inline bool GridMap::isInMap(const Eigen::Vector3d& pos) {
// if (pos(0) < mp_.map_min_boundary_(0) + 1e-4 || pos(1) < mp_.map_min_boundary_(1) + 1e-4 ||
// pos(2) < mp_.map_min_boundary_(2) + 1e-4) {
// // cout << "less than min range!" << endl;
// return false;
// }
// if (pos(0) > mp_.map_max_boundary_(0) - 1e-4 || pos(1) > mp_.map_max_boundary_(1) - 1e-4 ||
// pos(2) > mp_.map_max_boundary_(2) - 1e-4) {
// return false;
// }
// return true;
// }
// inline bool GridMap::isInMap(const Eigen::Vector3i& idx) {
// if (idx(0) < 0 || idx(1) < 0 || idx(2) < 0) {
// return false;
// }
// if (idx(0) > mp_.map_voxel_num_(0) - 1 || idx(1) > mp_.map_voxel_num_(1) - 1 ||
// idx(2) > mp_.map_voxel_num_(2) - 1) {
// return false;
// }
// return true;
// }
// inline void GridMap::posToIndex(const Eigen::Vector3d& pos, Eigen::Vector3i& id) {
// for (int i = 0; i < 3; ++i) id(i) = floor((pos(i) - mp_.map_origin_(i)) * mp_.resolution_inv_);
// }
// inline void GridMap::indexToPos(const Eigen::Vector3i& id, Eigen::Vector3d& pos) {
// for (int i = 0; i < 3; ++i) pos(i) = (id(i) + 0.5) * mp_.resolution_ + mp_.map_origin_(i);
// }
// inline void GridMap::inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts) {
// int num = 0;
// /* ---------- + shape inflate ---------- */
// // for (int x = -step; x <= step; ++x)
// // {
// // if (x == 0)
// // continue;
// // pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1), pt(2));
// // }
// // for (int y = -step; y <= step; ++y)
// // {
// // if (y == 0)
// // continue;
// // pts[num++] = Eigen::Vector3i(pt(0), pt(1) + y, pt(2));
// // }
// // for (int z = -1; z <= 1; ++z)
// // {
// // pts[num++] = Eigen::Vector3i(pt(0), pt(1), pt(2) + z);
// // }
// /* ---------- all inflate ---------- */
// for (int x = -step; x <= step; ++x)
// for (int y = -step; y <= step; ++y)
// for (int z = -step; z <= step; ++z) {
// pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z);
// }
// }
// inline double GridMap::getResolution() { return mp_.resolution_; }
// #endif

267
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_env/include/plan_env/linear_obj_model.hpp
Unescape View File

@ -1,267 +0,0 @@
/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef _LINEAR_OBJ_MODEL_H_
#define _LINEAR_OBJ_MODEL_H_
#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>
class LinearObjModel {
private:
bool last_out_bound_{false};
int input_type_;
/* data */
public:
LinearObjModel(/* args */);
~LinearObjModel();
void initialize(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw, double yaw_dot,
Eigen::Vector3d color, Eigen::Vector3d scale, int input_type);
void setLimits(Eigen::Vector3d bound, Eigen::Vector2d vel, Eigen::Vector2d acc);
void update(double dt); // linear trippler integrator model
static bool collide(LinearObjModel& obj1, LinearObjModel& obj2);
// void setInput(Eigen::Vector3d acc) {
// acc_ = acc;
// }
void setInput(Eigen::Vector3d vel) {
vel_ = vel;
}
void setYawDot(double yaw_dot) {
yaw_dot_ = yaw_dot;
}
Eigen::Vector3d getPosition() {
return pos_;
}
void setPosition(Eigen::Vector3d pos) {
pos_ = pos;
}
Eigen::Vector3d getVelocity() {
return vel_;
}
void setVelocity(double x, double y, double z) {
vel_ = Eigen::Vector3d(x, y, z);
}
Eigen::Vector3d getColor() {
return color_;
}
Eigen::Vector3d getScale() {
return scale_;
}
double getYaw() {
return yaw_;
}
private:
Eigen::Vector3d pos_, vel_, acc_;
Eigen::Vector3d color_, scale_;
double yaw_, yaw_dot_;
Eigen::Vector3d bound_;
Eigen::Vector2d limit_v_, limit_a_;
};
LinearObjModel::LinearObjModel(/* args */) {
}
LinearObjModel::~LinearObjModel() {
}
void LinearObjModel::initialize(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw,
double yaw_dot, Eigen::Vector3d color, Eigen::Vector3d scale, int input_type) {
pos_ = p;
vel_ = v;
acc_ = a;
color_ = color;
scale_ = scale;
input_type_ = input_type;
yaw_ = yaw;
yaw_dot_ = yaw_dot;
}
void LinearObjModel::setLimits(Eigen::Vector3d bound, Eigen::Vector2d vel, Eigen::Vector2d acc) {
bound_ = bound;
limit_v_ = vel;
limit_a_ = acc;
}
void LinearObjModel::update(double dt) {
Eigen::Vector3d p0, v0, a0;
p0 = pos_, v0 = vel_, a0 = acc_;
//std::cout << v0.transpose() << std::endl;
/* ---------- use acc as input ---------- */
if ( input_type_ == 2 )
{
vel_ = v0 + acc_ * dt;
for (int i = 0; i < 3; ++i)
{
if (vel_(i) > 0) vel_(i) = std::max(limit_v_(0), std::min(vel_(i),
limit_v_(1)));
if (vel_(i) <= 0) vel_(i) = std::max(-limit_v_(1), std::min(vel_(i),
-limit_v_(0)));
}
pos_ = p0 + v0 * dt + 0.5 * acc_ * pow(dt, 2);
/* ---------- reflect acc when collide with bound ---------- */
if ( pos_(0) <= bound_(0) && pos_(0) >= -bound_(0) &&
pos_(1) <= bound_(1) && pos_(1) >= -bound_(1) &&
pos_(2) <= bound_(2) && pos_(2) >= 0
)
{
last_out_bound_ = false;
}
else if ( !last_out_bound_ )
{
last_out_bound_ = true;
// if ( pos_(0) > bound_(0) || pos_(0) < -bound_(0) ) acc_(0) = -acc_(0);
// if ( pos_(1) > bound_(1) || pos_(1) < -bound_(1) ) acc_(1) = -acc_(1);
// if ( pos_(2) > bound_(2) || pos_(2) < -bound_(2) ) acc_(2) = -acc_(2);
acc_ = -acc_;
//ROS_ERROR("AAAAAAAAAAAAAAAAAAa");
}
}
// for (int i = 0; i < 2; ++i)
// {
// pos_(i) = std::min(pos_(i), bound_(i));
// pos_(i) = std::max(pos_(i), -bound_(i));
// }
// pos_(2) = std::min(pos_(2), bound_(2));
// pos_(2) = std::max(pos_(2), 0.0);
/* ---------- use vel as input ---------- */
else if ( input_type_ == 1 )
{
pos_ = p0 + v0 * dt;
for (int i = 0; i < 2; ++i) {
pos_(i) = std::min(pos_(i), bound_(i));
pos_(i) = std::max(pos_(i), -bound_(i));
}
pos_(2) = std::min(pos_(2), bound_(2));
pos_(2) = std::max(pos_(2), 0.0);
yaw_ += yaw_dot_ * dt;
const double PI = 3.1415926;
if (yaw_ > 2 * PI) yaw_ -= 2 * PI;
const double tol = 0.1;
if (pos_(0) > bound_(0) - tol) {
pos_(0) = bound_(0) - tol;
vel_(0) = -vel_(0);
}
if (pos_(0) < -bound_(0) + tol) {
pos_(0) = -bound_(0) + tol;
vel_(0) = -vel_(0);
}
if (pos_(1) > bound_(1) - tol) {
pos_(1) = bound_(1) - tol;
vel_(1) = -vel_(1);
}
if (pos_(1) < -bound_(1) + tol) {
pos_(1) = -bound_(1) + tol;
vel_(1) = -vel_(1);
}
if (pos_(2) > bound_(2) - tol) {
pos_(2) = bound_(2) - tol;
vel_(2) = -vel_(2);
}
if (pos_(2) < tol) {
pos_(2) = tol;
vel_(2) = -vel_(2);
}
}
// /* ---------- reflect when collide with bound ---------- */
//std::cout << pos_.transpose() << " " << bound_.transpose() << std::endl;
}
bool LinearObjModel::collide(LinearObjModel& obj1, LinearObjModel& obj2) {
Eigen::Vector3d pos1, pos2, vel1, vel2, scale1, scale2;
pos1 = obj1.getPosition();
vel1 = obj1.getVelocity();
scale1 = obj1.getScale();
pos2 = obj2.getPosition();
vel2 = obj2.getVelocity();
scale2 = obj2.getScale();
/* ---------- collide ---------- */
bool collide = fabs(pos1(0) - pos2(0)) < 0.5 * (scale1(0) + scale2(0)) &&
fabs(pos1(1) - pos2(1)) < 0.5 * (scale1(1) + scale2(1)) &&
fabs(pos1(2) - pos2(2)) < 0.5 * (scale1(2) + scale2(2));
if (collide) {
double tol[3];
tol[0] = 0.5 * (scale1(0) + scale2(0)) - fabs(pos1(0) - pos2(0));
tol[1] = 0.5 * (scale1(1) + scale2(1)) - fabs(pos1(1) - pos2(1));
tol[2] = 0.5 * (scale1(2) + scale2(2)) - fabs(pos1(2) - pos2(2));
for (int i = 0; i < 3; ++i) {
if (tol[i] < tol[(i + 1) % 3] && tol[i] < tol[(i + 2) % 3]) {
vel1(i) = -vel1(i);
vel2(i) = -vel2(i);
obj1.setVelocity(vel1(0), vel1(1), vel1(2));
obj2.setVelocity(vel2(0), vel2(1), vel2(2));
if (pos1(i) >= pos2(i)) {
pos1(i) += tol[i];
pos2(i) -= tol[i];
} else {
pos1(i) -= tol[i];
pos2(i) += tol[i];
}
obj1.setPosition(pos1);
obj2.setPosition(pos2);
break;
}
}
return true;
} else {
return false;
}
}
#endif

176
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_env/include/plan_env/obj_predictor.h
Unescape View File

@ -1,176 +0,0 @@
/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef _OBJ_PREDICTOR_H_
#define _OBJ_PREDICTOR_H_
#include <Eigen/Eigen>
#include <algorithm>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
#include <list>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
using std::cout;
using std::endl;
using std::list;
using std::shared_ptr;
using std::unique_ptr;
using std::vector;
namespace fast_planner {
class PolynomialPrediction;
typedef shared_ptr<vector<PolynomialPrediction>> ObjPrediction;
typedef shared_ptr<vector<Eigen::Vector3d>> ObjScale;
/* ========== prediction polynomial ========== */
class PolynomialPrediction {
private:
vector<Eigen::Matrix<double, 6, 1>> polys;
double t1, t2; // start / end
ros::Time global_start_time_;
public:
PolynomialPrediction(/* args */) {
}
~PolynomialPrediction() {
}
void setPolynomial(vector<Eigen::Matrix<double, 6, 1>>& pls) {
polys = pls;
}
void setTime(double t1, double t2) {
this->t1 = t1;
this->t2 = t2;
}
void setGlobalStartTime(ros::Time global_start_time) {
global_start_time_ = global_start_time;
}
bool valid() {
return polys.size() == 3;
}
/* note that t should be in [t1, t2] */
Eigen::Vector3d evaluate(double t) {
Eigen::Matrix<double, 6, 1> tv;
tv << 1.0, pow(t, 1), pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5);
Eigen::Vector3d pt;
pt(0) = tv.dot(polys[0]), pt(1) = tv.dot(polys[1]), pt(2) = tv.dot(polys[2]);
return pt;
}
Eigen::Vector3d evaluateConstVel(double t) {
Eigen::Matrix<double, 2, 1> tv;
tv << 1.0, pow(t-global_start_time_.toSec(), 1);
// cout << t-global_start_time_.toSec() << endl;
Eigen::Vector3d pt;
pt(0) = tv.dot(polys[0].head(2)), pt(1) = tv.dot(polys[1].head(2)), pt(2) = tv.dot(polys[2].head(2));
return pt;
}
};
/* ========== subscribe and record object history ========== */
class ObjHistory {
public:
int skip_num_;
int queue_size_;
ros::Time global_start_time_;
ObjHistory() {
}
~ObjHistory() {
}
void init(int id, int skip_num, int queue_size, ros::Time global_start_time);
void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
void clear() {
history_.clear();
}
void getHistory(list<Eigen::Vector4d>& his) {
his = history_;
}
private:
list<Eigen::Vector4d> history_; // x,y,z;t
int skip_;
int obj_idx_;
Eigen::Vector3d scale_;
};
/* ========== predict future trajectory using history ========== */
class ObjPredictor {
private:
ros::NodeHandle node_handle_;
int obj_num_;
double lambda_;
double predict_rate_;
vector<ros::Subscriber> pose_subs_;
ros::Subscriber marker_sub_;
ros::Timer predict_timer_;
vector<shared_ptr<ObjHistory>> obj_histories_;
/* share data with planner */
ObjPrediction predict_trajs_;
ObjScale obj_scale_;
vector<bool> scale_init_;
void markerCallback(const visualization_msgs::MarkerConstPtr& msg);
void predictCallback(const ros::TimerEvent& e);
void predictPolyFit();
void predictConstVel();
public:
ObjPredictor(/* args */);
ObjPredictor(ros::NodeHandle& node);
~ObjPredictor();
void init();
ObjPrediction getPredictionTraj();
ObjScale getObjScale();
int getObjNums() {return obj_num_;}
Eigen::Vector3d evaluatePoly(int obs_id, double time);
Eigen::Vector3d evaluateConstVel(int obs_id, double time);
typedef shared_ptr<ObjPredictor> Ptr;
};
} // namespace fast_planner
#endif

63
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_env/include/plan_env/raycast.h
Unescape View File

@ -1,63 +0,0 @@
#ifndef RAYCAST_H_
#define RAYCAST_H_
#include <Eigen/Eigen>
#include <vector>
double signum(double x);
double mod(double value, double modulus);
double intbound(double s, double ds);
void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output);
void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
const Eigen::Vector3d& max, std::vector<Eigen::Vector3d>* output);
class RayCaster {
private:
/* data */
Eigen::Vector3d start_;
Eigen::Vector3d end_;
Eigen::Vector3d direction_;
Eigen::Vector3d min_;
Eigen::Vector3d max_;
int x_;
int y_;
int z_;
int endX_;
int endY_;
int endZ_;
double maxDist_;
double dx_;
double dy_;
double dz_;
int stepX_;
int stepY_;
int stepZ_;
double tMaxX_;
double tMaxY_;
double tMaxZ_;
double tDeltaX_;
double tDeltaY_;
double tDeltaZ_;
double dist_;
int step_num_;
public:
RayCaster(/* args */) {
}
~RayCaster() {
}
bool setInput(const Eigen::Vector3d& start,
const Eigen::Vector3d& end /* , const Eigen::Vector3d& min,
const Eigen::Vector3d& max */);
bool step(Eigen::Vector3d& ray_pt);
};
#endif // RAYCAST_H_

68
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_env/package.xml
Unescape View File

@ -1,68 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>plan_env</name>
<version>0.0.0</version>
<description>The plan_env package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/plan_env</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

1122
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_env/src/grid_map.cpp
View File

File diff suppressed because it is too large Load Diff

238
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_env/src/obj_generator.cpp
Unescape View File

@ -1,238 +0,0 @@
/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/
#include "visualization_msgs/Marker.h"
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <random>
#include <sensor_msgs/PointCloud2.h>
#include <string>
#include <plan_env/linear_obj_model.hpp>
using namespace std;
int obj_num, _input_type;
double _x_size, _y_size, _h_size, _vel, _yaw_dot, _acc_r1, _acc_r2, _acc_z, _scale1, _scale2, _interval;
ros::Publisher obj_pub; // visualize marker
vector<ros::Publisher> pose_pubs; // obj pose (from optitrack)
vector<LinearObjModel> obj_models;
random_device rd;
default_random_engine eng(rd());
uniform_real_distribution<double> rand_pos_x;
uniform_real_distribution<double> rand_pos_y;
uniform_real_distribution<double> rand_h;
uniform_real_distribution<double> rand_vel;
uniform_real_distribution<double> rand_acc_r;
uniform_real_distribution<double> rand_acc_t;
uniform_real_distribution<double> rand_acc_z;
uniform_real_distribution<double> rand_color;
uniform_real_distribution<double> rand_scale;
uniform_real_distribution<double> rand_yaw_dot;
uniform_real_distribution<double> rand_yaw;
ros::Time time_update, time_change;
void updateCallback(const ros::TimerEvent& e);
void visualizeObj(int id);
int main(int argc, char** argv) {
ros::init(argc, argv, "dynamic_obj");
ros::NodeHandle node("~");
/* ---------- initialize ---------- */
node.param("obj_generator/obj_num", obj_num, 20);
node.param("obj_generator/x_size", _x_size, 10.0);
node.param("obj_generator/y_size", _y_size, 10.0);
node.param("obj_generator/h_size", _h_size, 2.0);
node.param("obj_generator/vel", _vel, 2.0);
node.param("obj_generator/yaw_dot", _yaw_dot, 2.0);
node.param("obj_generator/acc_r1", _acc_r1, 2.0);
node.param("obj_generator/acc_r2", _acc_r2, 2.0);
node.param("obj_generator/acc_z", _acc_z, 0.0);
node.param("obj_generator/scale1", _scale1, 0.5);
node.param("obj_generator/scale2", _scale2, 1.0);
node.param("obj_generator/interval", _interval, 100.0);
node.param("obj_generator/input_type", _input_type, 1);
obj_pub = node.advertise<visualization_msgs::Marker>("/dynamic/obj", 10);
for (int i = 0; i < obj_num; ++i) {
ros::Publisher pose_pub =
node.advertise<geometry_msgs::PoseStamped>("/dynamic/pose_" + to_string(i), 10);
pose_pubs.push_back(pose_pub);
}
ros::Timer update_timer = node.createTimer(ros::Duration(1 / 30.0), updateCallback);
cout << "[dynamic]: initialize with " + to_string(obj_num) << " moving obj." << endl;
ros::Duration(1.0).sleep();
rand_color = uniform_real_distribution<double>(0.0, 1.0);
rand_pos_x = uniform_real_distribution<double>(-_x_size/2, _x_size/2);
rand_pos_y = uniform_real_distribution<double>(-_y_size/2, _y_size/2);
rand_h = uniform_real_distribution<double>(0.0, _h_size);
rand_vel = uniform_real_distribution<double>(-_vel, _vel);
rand_acc_t = uniform_real_distribution<double>(0.0, 6.28);
rand_acc_r = uniform_real_distribution<double>(_acc_r1, _acc_r2);
rand_acc_z = uniform_real_distribution<double>(-_acc_z, _acc_z);
rand_scale = uniform_real_distribution<double>(_scale1, _scale2);
rand_yaw = uniform_real_distribution<double>(0.0, 2 * 3.141592);
rand_yaw_dot = uniform_real_distribution<double>(-_yaw_dot, _yaw_dot);
/* ---------- give initial value of each obj ---------- */
for (int i = 0; i < obj_num; ++i) {
LinearObjModel model;
Eigen::Vector3d pos(rand_pos_x(eng), rand_pos_y(eng), rand_h(eng));
Eigen::Vector3d vel(rand_vel(eng), rand_vel(eng), 0.0);
Eigen::Vector3d color(rand_color(eng), rand_color(eng), rand_color(eng));
Eigen::Vector3d scale(rand_scale(eng), 1.5 * rand_scale(eng), 5.0*rand_scale(eng));
double yaw = rand_yaw(eng);
double yaw_dot = rand_yaw_dot(eng);
double r, t, z;
r = rand_acc_r(eng);
t = rand_acc_t(eng);
z = rand_acc_z(eng);
Eigen::Vector3d acc(r * cos(t), r * sin(t), z);
if ( _input_type == 1 )
{
model.initialize(pos, vel, acc, yaw, yaw_dot, color, scale, _input_type); // Vel input
}
else
{
model.initialize(pos, Eigen::Vector3d(0,0,0), acc, yaw, yaw_dot, color, scale, _input_type); // Acc input
}
model.setLimits(Eigen::Vector3d(_x_size/2, _y_size/2, _h_size), Eigen::Vector2d(0.0, _vel),
Eigen::Vector2d(0, 0));
obj_models.push_back(model);
}
time_update = ros::Time::now();
time_change = ros::Time::now();
/* ---------- start loop ---------- */
ros::spin();
return 0;
}
void updateCallback(const ros::TimerEvent& e) {
ros::Time time_now = ros::Time::now();
/* ---------- change input ---------- */
// double dtc = (time_now - time_change).toSec();
// if (dtc > _interval) {
// for (int i = 0; i < obj_num; ++i) {
// /* ---------- use acc input ---------- */
// // double r, t, z;
// // r = rand_acc_r(eng);
// // t = rand_acc_t(eng);
// // z = rand_acc_z(eng);
// // Eigen::Vector3d acc(r * cos(t), r * sin(t), z);
// // obj_models[i].setInput(acc);
// /* ---------- use vel input ---------- */
// double vx, vy, vz, yd;
// vx = rand_vel(eng);
// vy = rand_vel(eng);
// vz = 0.0;
// yd = rand_yaw_dot(eng);
// obj_models[i].setInput(Eigen::Vector3d(vx, vy, vz));
// obj_models[i].setYawDot(yd);
// }
// time_change = time_now;
// }
/* ---------- update obj state ---------- */
double dt = (time_now - time_update).toSec();
time_update = time_now;
for (int i = 0; i < obj_num; ++i) {
obj_models[i].update(dt);
visualizeObj(i);
ros::Duration(0.000001).sleep();
}
/* ---------- collision ---------- */
// for (int i = 0; i < obj_num; ++i)
// for (int j = i + 1; j < obj_num; ++j) {
// bool collision = LinearObjModel::collide(obj_models[i], obj_models[j]);
// if (collision) {
// double yd1 = rand_yaw_dot(eng);
// double yd2 = rand_yaw_dot(eng);
// obj_models[i].setYawDot(yd1);
// obj_models[j].setYawDot(yd2);
// }
// }
}
void visualizeObj(int id) {
Eigen::Vector3d pos, color, scale;
pos = obj_models[id].getPosition();
color = obj_models[id].getColor();
scale = obj_models[id].getScale();
double yaw = obj_models[id].getYaw();
Eigen::Matrix3d rot;
rot << cos(yaw), -sin(yaw), 0.0, sin(yaw), cos(yaw), 0.0, 0.0, 0.0, 1.0;
Eigen::Quaterniond qua;
qua = rot;
/* ---------- rviz ---------- */
visualization_msgs::Marker mk;
mk.header.frame_id = "world";
mk.header.stamp = ros::Time::now();
mk.type = visualization_msgs::Marker::CUBE;
mk.action = visualization_msgs::Marker::ADD;
mk.id = id;
mk.scale.x = scale(0), mk.scale.y = scale(1), mk.scale.z = scale(2);
mk.color.a = 1.0, mk.color.r = color(0), mk.color.g = color(1), mk.color.b = color(2);
mk.pose.orientation.w = qua.w();
mk.pose.orientation.x = qua.x();
mk.pose.orientation.y = qua.y();
mk.pose.orientation.z = qua.z();
mk.pose.position.x = pos(0), mk.pose.position.y = pos(1), mk.pose.position.z = pos(2);
obj_pub.publish(mk);
/* ---------- pose ---------- */
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "world";
pose.header.seq = id;
pose.pose.position.x = pos(0), pose.pose.position.y = pos(1), pose.pose.position.z = pos(2);
pose.pose.orientation.w = 1.0;
pose_pubs[id].publish(pose);
}

289
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_env/src/obj_predictor.cpp
Unescape View File

@ -1,289 +0,0 @@
/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/
#include <plan_env/obj_predictor.h>
#include <string>
namespace fast_planner {
/* ============================== obj history_ ============================== */
// int ObjHistory::queue_size_;
// int ObjHistory::skip_num_;
// ros::Time ObjHistory::global_start_time_;
void ObjHistory::init(int id, int skip_num, int queue_size, ros::Time global_start_time) {
clear();
skip_ = 0;
obj_idx_ = id;
skip_num_ = skip_num;
queue_size_ = queue_size;
global_start_time_ = global_start_time;
}
void ObjHistory::poseCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
++skip_;
if (skip_ < skip_num_) return;
Eigen::Vector4d pos_t;
pos_t(0) = msg->pose.position.x, pos_t(1) = msg->pose.position.y, pos_t(2) = msg->pose.position.z;
pos_t(3) = (ros::Time::now() - global_start_time_).toSec();
history_.push_back(pos_t);
// cout << "idx: " << obj_idx_ << "pos_t: " << pos_t.transpose() << endl;
if (history_.size() > queue_size_) history_.pop_front();
skip_ = 0;
}
// ObjHistory::
/* ============================== obj predictor ==============================
*/
ObjPredictor::ObjPredictor(/* args */) {
}
ObjPredictor::ObjPredictor(ros::NodeHandle& node) {
this->node_handle_ = node;
}
ObjPredictor::~ObjPredictor() {
}
void ObjPredictor::init() {
/* get param */
int queue_size, skip_nums;
node_handle_.param("prediction/obj_num", obj_num_, 0);
node_handle_.param("prediction/lambda", lambda_, 1.0);
node_handle_.param("prediction/predict_rate", predict_rate_, 1.0);
node_handle_.param("prediction/queue_size", queue_size, 10);
node_handle_.param("prediction/skip_nums", skip_nums, 1);
predict_trajs_.reset(new vector<PolynomialPrediction>);
predict_trajs_->resize(obj_num_);
obj_scale_.reset(new vector<Eigen::Vector3d>);
obj_scale_->resize(obj_num_);
scale_init_.resize(obj_num_);
for (int i = 0; i < obj_num_; i++)
scale_init_[i] = false;
/* subscribe to pose */
ros::Time t_now = ros::Time::now();
for (int i = 0; i < obj_num_; i++) {
shared_ptr<ObjHistory> obj_his(new ObjHistory);
obj_his->init(i, skip_nums, queue_size, t_now);
obj_histories_.push_back(obj_his);
ros::Subscriber pose_sub = node_handle_.subscribe<geometry_msgs::PoseStamped>(
"/dynamic/pose_" + std::to_string(i), 10, &ObjHistory::poseCallback, obj_his.get());
pose_subs_.push_back(pose_sub);
predict_trajs_->at(i).setGlobalStartTime(t_now);
}
marker_sub_ = node_handle_.subscribe<visualization_msgs::Marker>("/dynamic/obj", 10,
&ObjPredictor::markerCallback, this);
/* update prediction */
predict_timer_ =
node_handle_.createTimer(ros::Duration(1 / predict_rate_), &ObjPredictor::predictCallback, this);
}
ObjPrediction ObjPredictor::getPredictionTraj() {
return this->predict_trajs_;
}
ObjScale ObjPredictor::getObjScale() {
return this->obj_scale_;
}
void ObjPredictor::predictPolyFit() {
/* iterate all obj */
for (int i = 0; i < obj_num_; i++) {
/* ---------- write A and b ---------- */
Eigen::Matrix<double, 6, 6> A;
Eigen::Matrix<double, 6, 1> temp;
Eigen::Matrix<double, 6, 1> bm[3]; // poly coefficent
vector<Eigen::Matrix<double, 6, 1>> pm(3);
A.setZero();
for (int i = 0; i < 3; ++i)
bm[i].setZero();
/* ---------- estimation error ---------- */
list<Eigen::Vector4d> his;
obj_histories_[i]->getHistory(his);
for (list<Eigen::Vector4d>::iterator it = his.begin(); it != his.end(); ++it) {
Eigen::Vector3d qi = (*it).head(3);
double ti = (*it)(3);
/* A */
temp << 1.0, ti, pow(ti, 2), pow(ti, 3), pow(ti, 4), pow(ti, 5);
for (int j = 0; j < 6; ++j)
A.row(j) += 2.0 * pow(ti, j) * temp.transpose();
/* b */
for (int dim = 0; dim < 3; ++dim)
bm[dim] += 2.0 * qi(dim) * temp;
}
/* ---------- acceleration regulator ---------- */
double t1 = his.front()(3);
double t2 = his.back()(3);
temp << 0.0, 0.0, 2 * t1 - 2 * t2, 3 * pow(t1, 2) - 3 * pow(t2, 2), 4 * pow(t1, 3) - 4 * pow(t2, 3),
5 * pow(t1, 4) - 5 * pow(t2, 4);
A.row(2) += -4 * lambda_ * temp.transpose();
temp << 0.0, 0.0, pow(t1, 2) - pow(t2, 2), 2 * pow(t1, 3) - 2 * pow(t2, 3),
3 * pow(t1, 4) - 3 * pow(t2, 4), 4 * pow(t1, 5) - 4 * pow(t2, 5);
A.row(3) += -12 * lambda_ * temp.transpose();
temp << 0.0, 0.0, 20 * pow(t1, 3) - 20 * pow(t2, 3), 45 * pow(t1, 4) - 45 * pow(t2, 4),
72 * pow(t1, 5) - 72 * pow(t2, 5), 100 * pow(t1, 6) - 100 * pow(t2, 6);
A.row(4) += -4.0 / 5.0 * lambda_ * temp.transpose();
temp << 0.0, 0.0, 35 * pow(t1, 4) - 35 * pow(t2, 4), 84 * pow(t1, 5) - 84 * pow(t2, 5),
140 * pow(t1, 6) - 140 * pow(t2, 6), 200 * pow(t1, 7) - 200 * pow(t2, 7);
A.row(5) += -4.0 / 7.0 * lambda_ * temp.transpose();
/* ---------- solve ---------- */
for (int j = 0; j < 3; j++) {
pm[j] = A.colPivHouseholderQr().solve(bm[j]);
}
/* ---------- update prediction container ---------- */
predict_trajs_->at(i).setPolynomial(pm);
predict_trajs_->at(i).setTime(t1, t2);
}
}
void ObjPredictor::predictCallback(const ros::TimerEvent& e) {
// predictPolyFit();
predictConstVel();
}
void ObjPredictor::markerCallback(const visualization_msgs::MarkerConstPtr& msg) {
int idx = msg->id;
(*obj_scale_)[idx](0) = msg->scale.x;
(*obj_scale_)[idx](1) = msg->scale.y;
(*obj_scale_)[idx](2) = msg->scale.z;
scale_init_[idx] = true;
int finish_num = 0;
for (int i = 0; i < obj_num_; i++) {
if (scale_init_[i]) finish_num++;
}
if (finish_num == obj_num_) {
marker_sub_.shutdown();
}
}
void ObjPredictor::predictConstVel() {
for (int i = 0; i < obj_num_; i++) {
/* ---------- get the last two point ---------- */
list<Eigen::Vector4d> his;
obj_histories_[i]->getHistory(his);
// if ( i==0 )
// {
// cout << "his.size()=" << his.size() << endl;
// for ( auto hi:his )
// {
// cout << hi.transpose() << endl;
// }
// }
list<Eigen::Vector4d>::iterator list_it = his.end();
/* ---------- test iteration ---------- */
// cout << "----------------------------" << endl;
// for (auto v4d : his)
// cout << "v4d: " << v4d.transpose() << endl;
Eigen::Vector3d q1, q2;
double t1, t2;
--list_it;
q2 = (*list_it).head(3);
t2 = (*list_it)(3);
--list_it;
q1 = (*list_it).head(3);
t1 = (*list_it)(3);
Eigen::Matrix<double, 2, 3> p01, q12;
q12.row(0) = q1.transpose();
q12.row(1) = q2.transpose();
Eigen::Matrix<double, 2, 2> At12;
At12 << 1, t1, 1, t2;
p01 = At12.inverse() * q12;
vector<Eigen::Matrix<double, 6, 1>> polys(3);
for (int j = 0; j < 3; ++j) {
polys[j].setZero();
polys[j].head(2) = p01.col(j);
}
// if ( i==0 )
// {
// cout << "q1=" << q1.transpose() << " t1=" << t1 << " q2=" << q2.transpose() << " t2=" << t2 << endl;
// cout << "polys=" << polys[0].transpose() << endl;
// }
predict_trajs_->at(i).setPolynomial(polys);
predict_trajs_->at(i).setTime(t1, t2);
}
}
Eigen::Vector3d ObjPredictor::evaluatePoly(int obj_id, double time)
{
if ( obj_id < obj_num_ )
{
return predict_trajs_->at(obj_id).evaluate(time);
}
double MAX = std::numeric_limits<double>::max();
return Eigen::Vector3d(MAX, MAX, MAX);
}
Eigen::Vector3d ObjPredictor::evaluateConstVel(int obj_id, double time)
{
if ( obj_id < obj_num_ )
{
return predict_trajs_->at(obj_id).evaluateConstVel(time);
}
double MAX = std::numeric_limits<double>::max();
return Eigen::Vector3d(MAX, MAX, MAX);
}
// ObjPredictor::
} // namespace fast_planner

321
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_env/src/raycast.cpp
Unescape View File

@ -1,321 +0,0 @@
#include <Eigen/Eigen>
#include <cmath>
#include <iostream>
#include <plan_env/raycast.h>
int signum(int x) {
return x == 0 ? 0 : x < 0 ? -1 : 1;
}
double mod(double value, double modulus) {
return fmod(fmod(value, modulus) + modulus, modulus);
}
double intbound(double s, double ds) {
// Find the smallest positive t such that s+t*ds is an integer.
if (ds < 0) {
return intbound(-s, -ds);
} else {
s = mod(s, 1);
// problem is now s+t*ds = 1
return (1 - s) / ds;
}
}
void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output) {
// std::cout << start << ' ' << end << std::endl;
// From "A Fast Voxel Traversal Algorithm for Ray Tracing"
// by John Amanatides and Andrew Woo, 1987
// <http://www.cse.yorku.ca/~amana/research/grid.pdf>
// <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
// Extensions to the described algorithm:
// • Imposed a distance limit.
// • The face passed through to reach the current cube is provided to
// the callback.
// The foundation of this algorithm is a parameterized representation of
// the provided ray,
// origin + t * direction,
// except that t is not actually stored; rather, at any given point in the
// traversal, we keep track of the *greater* t values which we would have
// if we took a step sufficient to cross a cube boundary along that axis
// (i.e. change the integer part of the coordinate) in the variables
// tMaxX, tMaxY, and tMaxZ.
// Cube containing origin point.
int x = (int)std::floor(start.x());
int y = (int)std::floor(start.y());
int z = (int)std::floor(start.z());
int endX = (int)std::floor(end.x());
int endY = (int)std::floor(end.y());
int endZ = (int)std::floor(end.z());
Eigen::Vector3d direction = (end - start);
double maxDist = direction.squaredNorm();
// Break out direction vector.
double dx = endX - x;
double dy = endY - y;
double dz = endZ - z;
// Direction to increment x,y,z when stepping.
int stepX = (int)signum((int)dx);
int stepY = (int)signum((int)dy);
int stepZ = (int)signum((int)dz);
// See description above. The initial values depend on the fractional
// part of the origin.
double tMaxX = intbound(start.x(), dx);
double tMaxY = intbound(start.y(), dy);
double tMaxZ = intbound(start.z(), dz);
// The change in t when taking a step (always positive).
double tDeltaX = ((double)stepX) / dx;
double tDeltaY = ((double)stepY) / dy;
double tDeltaZ = ((double)stepZ) / dz;
// Avoids an infinite loop.
if (stepX == 0 && stepY == 0 && stepZ == 0) return;
double dist = 0;
while (true) {
if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) {
output[output_points_cnt](0) = x;
output[output_points_cnt](1) = y;
output[output_points_cnt](2) = z;
output_points_cnt++;
dist = sqrt((x - start(0)) * (x - start(0)) + (y - start(1)) * (y - start(1)) +
(z - start(2)) * (z - start(2)));
if (dist > maxDist) return;
/* if (output_points_cnt > 1500) {
std::cerr << "Error, too many racyast voxels." <<
std::endl;
throw std::out_of_range("Too many raycast voxels");
}*/
}
if (x == endX && y == endY && z == endZ) break;
// tMaxX stores the t-value at which we cross a cube boundary along the
// X axis, and similarly for Y and Z. Therefore, choosing the least tMax
// chooses the closest cube boundary. Only the first case of the four
// has been commented in detail.
if (tMaxX < tMaxY) {
if (tMaxX < tMaxZ) {
// Update which cube we are now in.
x += stepX;
// Adjust tMaxX to the next X-oriented boundary crossing.
tMaxX += tDeltaX;
} else {
z += stepZ;
tMaxZ += tDeltaZ;
}
} else {
if (tMaxY < tMaxZ) {
y += stepY;
tMaxY += tDeltaY;
} else {
z += stepZ;
tMaxZ += tDeltaZ;
}
}
}
}
void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min,
const Eigen::Vector3d& max, std::vector<Eigen::Vector3d>* output) {
// std::cout << start << ' ' << end << std::endl;
// From "A Fast Voxel Traversal Algorithm for Ray Tracing"
// by John Amanatides and Andrew Woo, 1987
// <http://www.cse.yorku.ca/~amana/research/grid.pdf>
// <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
// Extensions to the described algorithm:
// • Imposed a distance limit.
// • The face passed through to reach the current cube is provided to
// the callback.
// The foundation of this algorithm is a parameterized representation of
// the provided ray,
// origin + t * direction,
// except that t is not actually stored; rather, at any given point in the
// traversal, we keep track of the *greater* t values which we would have
// if we took a step sufficient to cross a cube boundary along that axis
// (i.e. change the integer part of the coordinate) in the variables
// tMaxX, tMaxY, and tMaxZ.
// Cube containing origin point.
int x = (int)std::floor(start.x());
int y = (int)std::floor(start.y());
int z = (int)std::floor(start.z());
int endX = (int)std::floor(end.x());
int endY = (int)std::floor(end.y());
int endZ = (int)std::floor(end.z());
Eigen::Vector3d direction = (end - start);
double maxDist = direction.squaredNorm();
// Break out direction vector.
double dx = endX - x;
double dy = endY - y;
double dz = endZ - z;
// Direction to increment x,y,z when stepping.
int stepX = (int)signum((int)dx);
int stepY = (int)signum((int)dy);
int stepZ = (int)signum((int)dz);
// See description above. The initial values depend on the fractional
// part of the origin.
double tMaxX = intbound(start.x(), dx);
double tMaxY = intbound(start.y(), dy);
double tMaxZ = intbound(start.z(), dz);
// The change in t when taking a step (always positive).
double tDeltaX = ((double)stepX) / dx;
double tDeltaY = ((double)stepY) / dy;
double tDeltaZ = ((double)stepZ) / dz;
output->clear();
// Avoids an infinite loop.
if (stepX == 0 && stepY == 0 && stepZ == 0) return;
double dist = 0;
while (true) {
if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() && z >= min.z() && z < max.z()) {
output->push_back(Eigen::Vector3d(x, y, z));
dist = (Eigen::Vector3d(x, y, z) - start).squaredNorm();
if (dist > maxDist) return;
if (output->size() > 1500) {
std::cerr << "Error, too many racyast voxels." << std::endl;
throw std::out_of_range("Too many raycast voxels");
}
}
if (x == endX && y == endY && z == endZ) break;
// tMaxX stores the t-value at which we cross a cube boundary along the
// X axis, and similarly for Y and Z. Therefore, choosing the least tMax
// chooses the closest cube boundary. Only the first case of the four
// has been commented in detail.
if (tMaxX < tMaxY) {
if (tMaxX < tMaxZ) {
// Update which cube we are now in.
x += stepX;
// Adjust tMaxX to the next X-oriented boundary crossing.
tMaxX += tDeltaX;
} else {
z += stepZ;
tMaxZ += tDeltaZ;
}
} else {
if (tMaxY < tMaxZ) {
y += stepY;
tMaxY += tDeltaY;
} else {
z += stepZ;
tMaxZ += tDeltaZ;
}
}
}
}
bool RayCaster::setInput(const Eigen::Vector3d& start,
const Eigen::Vector3d& end /* , const Eigen::Vector3d& min,
const Eigen::Vector3d& max */) {
start_ = start;
end_ = end;
// max_ = max;
// min_ = min;
x_ = (int)std::floor(start_.x());
y_ = (int)std::floor(start_.y());
z_ = (int)std::floor(start_.z());
endX_ = (int)std::floor(end_.x());
endY_ = (int)std::floor(end_.y());
endZ_ = (int)std::floor(end_.z());
direction_ = (end_ - start_);
maxDist_ = direction_.squaredNorm();
// Break out direction vector.
dx_ = endX_ - x_;
dy_ = endY_ - y_;
dz_ = endZ_ - z_;
// Direction to increment x,y,z when stepping.
stepX_ = (int)signum((int)dx_);
stepY_ = (int)signum((int)dy_);
stepZ_ = (int)signum((int)dz_);
// See description above. The initial values depend on the fractional
// part of the origin.
tMaxX_ = intbound(start_.x(), dx_);
tMaxY_ = intbound(start_.y(), dy_);
tMaxZ_ = intbound(start_.z(), dz_);
// The change in t when taking a step (always positive).
tDeltaX_ = ((double)stepX_) / dx_;
tDeltaY_ = ((double)stepY_) / dy_;
tDeltaZ_ = ((double)stepZ_) / dz_;
dist_ = 0;
step_num_ = 0;
// Avoids an infinite loop.
if (stepX_ == 0 && stepY_ == 0 && stepZ_ == 0)
return false;
else
return true;
}
bool RayCaster::step(Eigen::Vector3d& ray_pt) {
// if (x_ >= min_.x() && x_ < max_.x() && y_ >= min_.y() && y_ < max_.y() &&
// z_ >= min_.z() && z_ <
// max_.z())
ray_pt = Eigen::Vector3d(x_, y_, z_);
// step_num_++;
// dist_ = (Eigen::Vector3d(x_, y_, z_) - start_).squaredNorm();
if (x_ == endX_ && y_ == endY_ && z_ == endZ_) {
return false;
}
// if (dist_ > maxDist_)
// {
// return false;
// }
// tMaxX stores the t-value at which we cross a cube boundary along the
// X axis, and similarly for Y and Z. Therefore, choosing the least tMax
// chooses the closest cube boundary. Only the first case of the four
// has been commented in detail.
if (tMaxX_ < tMaxY_) {
if (tMaxX_ < tMaxZ_) {
// Update which cube we are now in.
x_ += stepX_;
// Adjust tMaxX to the next X-oriented boundary crossing.
tMaxX_ += tDeltaX_;
} else {
z_ += stepZ_;
tMaxZ_ += tDeltaZ_;
}
} else {
if (tMaxY_ < tMaxZ_) {
y_ += stepY_;
tMaxY_ += tDeltaY_;
} else {
z_ += stepZ_;
tMaxZ_ += tDeltaZ_;
}
}
return true;
}

55
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/CMakeLists.txt
Unescape View File

@ -1,55 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(ego_planner)
set(CMAKE_BUILD_TYPE "Release")
ADD_COMPILE_OPTIONS(-std=c++11 )
ADD_COMPILE_OPTIONS(-std=c++14 )
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
find_package(Eigen3 REQUIRED)
find_package(PCL 1.7 REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
geometry_msgs
quadrotor_msgs
plan_env
path_searching
bspline_opt
traj_utils
message_generation
cv_bridge
prometheus_msgs
)
# catkin_package(CATKIN_DEPENDS message_runtime)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ego_planner
CATKIN_DEPENDS plan_env path_searching bspline_opt traj_utils
# DEPENDS system_lib
)
include_directories(
include
SYSTEM
${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIR}
${PCL_INCLUDE_DIRS}
)
add_executable(ego_planner_node
src/ego_planner_node.cpp
src/ego_replan_fsm.cpp
src/planner_manager.cpp
)
target_link_libraries(ego_planner_node
${catkin_LIBRARIES}
)
#add_dependencies(ego_planner_node ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_executable(traj_server src/traj_server.cpp)
target_link_libraries(traj_server ${catkin_LIBRARIES})
#add_dependencies(traj_server ${${PROJECT_NAME}_EXPORTED_TARGETS})

132
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/include/plan_manage/ego_replan_fsm.h
Unescape View File

@ -1,132 +0,0 @@
#ifndef _REBO_REPLAN_FSM_H_
#define _REBO_REPLAN_FSM_H_
#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>
#include <nav_msgs/Path.h>
#include <sensor_msgs/Imu.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <vector>
#include <visualization_msgs/Marker.h>
#include <bspline_opt/bspline_optimizer.h>
#include <plan_env/grid_map.h>
#include <traj_utils/Bspline.h>
#include <traj_utils/MultiBsplines.h>
#include <geometry_msgs/PoseStamped.h>
#include <traj_utils/DataDisp.h>
#include <plan_manage/planner_manager.h>
#include <traj_utils/planning_visualization.h>
using std::vector;
namespace ego_planner
{
class EGOReplanFSM
{
private:
/* ---------- flag ---------- */
//规划器状态
enum FSM_EXEC_STATE
{
INIT,
WAIT_TARGET,
GEN_NEW_TRAJ,
REPLAN_TRAJ,
EXEC_TRAJ,
EMERGENCY_STOP,
SEQUENTIAL_START
};
// 目标点类型
enum TARGET_TYPE
{
MANUAL_TARGET = 1,
PRESET_TARGET = 2,
REFENCE_PATH = 3
};
/* planning utils */
EGOPlannerManager::Ptr planner_manager_;
PlanningVisualization::Ptr visualization_;
traj_utils::DataDisp data_disp_;
traj_utils::MultiBsplines multi_bspline_msgs_buf_;
/* parameters */
int target_type_; // 1 mannual select, 2 hard code
double no_replan_thresh_, replan_thresh_;
double waypoints_[50][3];
int waypoint_num_, wp_id_;
double planning_horizen_, planning_horizen_time_;
double emergency_time_;
bool flag_realworld_experiment_;
bool enable_fail_safe_;
/* planning data */
bool have_trigger_, have_target_, have_odom_, have_new_target_, have_recv_pre_agent_;
FSM_EXEC_STATE exec_state_;
int continously_called_times_{0};
Eigen::Vector3d odom_pos_, odom_vel_, odom_acc_; // odometry state
Eigen::Quaterniond odom_orient_;
Eigen::Vector3d init_pt_, start_pt_, start_vel_, start_acc_, start_yaw_; // start state
Eigen::Vector3d end_pt_, end_vel_; // goal state
Eigen::Vector3d local_target_pt_, local_target_vel_; // local target state
std::vector<Eigen::Vector3d> wps_;
int current_wp_;
bool flag_escape_emergency_;
/* ROS utils */
ros::NodeHandle node_;
ros::Timer exec_timer_, safety_timer_;
ros::Subscriber waypoint_sub_, odom_sub_, swarm_trajs_sub_, broadcast_bspline_sub_, trigger_sub_;
ros::Publisher replan_pub_, new_pub_, bspline_pub_, data_disp_pub_, swarm_trajs_pub_, broadcast_bspline_pub_;
/* helper functions */
bool callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj); // front-end and back-end method
bool callEmergencyStop(Eigen::Vector3d stop_pos); // front-end and back-end method
bool planFromGlobalTraj(const int trial_times = 1);
bool planFromCurrentTraj(const int trial_times = 1);
/* return value: std::pair< Times of the same state be continuously called, current continuously called state > */
void changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call);
std::pair<int, EGOReplanFSM::FSM_EXEC_STATE> timesOfConsecutiveStateCalls();
void printFSMExecState();
void readGivenWps();
void planNextWaypoint(const Eigen::Vector3d next_wp);
void getLocalTarget();
/* ROS functions */
void execFSMCallback(const ros::TimerEvent &e);
void checkCollisionCallback(const ros::TimerEvent &e);
void waypointCallback(const geometry_msgs::PoseStampedPtr &msg);
void triggerCallback(const geometry_msgs::PoseStampedPtr &msg);
void odometryCallback(const nav_msgs::OdometryConstPtr &msg);
void swarmTrajsCallback(const traj_utils::MultiBsplinesPtr &msg);
void BroadcastBsplineCallback(const traj_utils::BsplinePtr &msg);
bool checkCollision();
void publishSwarmTrajs(bool startup_pub);
public:
EGOReplanFSM(/* args */)
{
}
~EGOReplanFSM()
{
}
void init(ros::NodeHandle &nh);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace ego_planner
#endif

85
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/include/plan_manage/planner_manager.h
Unescape View File

@ -1,85 +0,0 @@
#ifndef _PLANNER_MANAGER_H_
#define _PLANNER_MANAGER_H_
#include <stdlib.h>
#include <bspline_opt/bspline_optimizer.h>
#include <bspline_opt/uniform_bspline.h>
#include <traj_utils/DataDisp.h>
#include <plan_env/grid_map.h>
#include <plan_env/obj_predictor.h>
#include <traj_utils/plan_container.hpp>
#include <ros/ros.h>
#include <traj_utils/planning_visualization.h>
namespace ego_planner
{
// Fast Planner Manager
// Key algorithms of mapping and planning are called
class EGOPlannerManager
{
// SECTION stable
public:
EGOPlannerManager();
~EGOPlannerManager();
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* main planning interface */
bool reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel, Eigen::Vector3d start_acc,
Eigen::Vector3d end_pt, Eigen::Vector3d end_vel, bool flag_polyInit, bool flag_randomPolyTraj);
bool EmergencyStop(Eigen::Vector3d stop_pos);
bool planGlobalTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
const Eigen::Vector3d &end_pos, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc);
bool planGlobalTrajWaypoints(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
const std::vector<Eigen::Vector3d> &waypoints, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc);
void initPlanModules(ros::NodeHandle &nh, PlanningVisualization::Ptr vis = NULL);
void deliverTrajToOptimizer(void) { bspline_optimizer_->setSwarmTrajs(&swarm_trajs_buf_); };
void setDroneIdtoOpt(void) { bspline_optimizer_->setDroneId(pp_.drone_id); }
double getSwarmClearance(void) { return bspline_optimizer_->getSwarmClearance(); }
bool checkCollision(int drone_id);
PlanParameters pp_;
LocalTrajData local_data_;
GlobalTrajData global_data_;
GridMap::Ptr grid_map_;
fast_planner::ObjPredictor::Ptr obj_predictor_;
SwarmTrajData swarm_trajs_buf_;
private:
/* main planning algorithms & modules */
PlanningVisualization::Ptr visualization_;
// ros::Publisher obj_pub_; //zx-todo
BsplineOptimizer::Ptr bspline_optimizer_;
int continous_failures_count_{0};
void updateTrajInfo(const UniformBspline &position_traj, const ros::Time time_now);
void reparamBspline(UniformBspline &bspline, vector<Eigen::Vector3d> &start_end_derivative, double ratio, Eigen::MatrixXd &ctrl_pts, double &dt,
double &time_inc);
bool refineTrajAlgo(UniformBspline &traj, vector<Eigen::Vector3d> &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points);
// !SECTION stable
// SECTION developing
public:
typedef unique_ptr<EGOPlannerManager> Ptr;
// !SECTION
};
} // namespace ego_planner
#endif

160
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/launch/advanced_param.xml
Unescape View File

@ -1,160 +0,0 @@
<launch>
<arg name="map_size_x_"/>
<arg name="map_size_y_"/>
<arg name="map_size_z_"/>
<arg name="odometry_topic"/>
<arg name="camera_pose_topic"/>
<arg name="depth_topic"/>
<arg name="cloud_topic"/>
<arg name="cx"/>
<arg name="cy"/>
<arg name="fx"/>
<arg name="fy"/>
<arg name="max_vel"/>
<arg name="max_acc"/>
<arg name="planning_horizon"/>
<arg name="point_num"/>
<arg name="point0_x"/>
<arg name="point0_y"/>
<arg name="point0_z"/>
<arg name="point1_x"/>
<arg name="point1_y"/>
<arg name="point1_z"/>
<arg name="point2_x"/>
<arg name="point2_y"/>
<arg name="point2_z"/>
<arg name="point3_x"/>
<arg name="point3_y"/>
<arg name="point3_z"/>
<arg name="point4_x"/>
<arg name="point4_y"/>
<arg name="point4_z"/>
<arg name="flight_type"/>
<arg name="use_distinctive_trajs"/>
<arg name="obj_num_set"/>
<arg name="drone_id"/>
<!-- main node -->
<!-- <node pkg="ego_planner" name="ego_planner_node" type="ego_planner_node" output="screen" launch-prefix="valgrind"> -->
<node pkg="ego_planner" name="drone_$(arg drone_id)_ego_planner_node" type="ego_planner_node" output="screen">
<remap from="~odom_world" to="/drone_$(arg drone_id)_$(arg odometry_topic)"/>
<remap from="~planning/bspline" to = "/drone_$(arg drone_id)_planning/bspline"/>
<remap from="~planning/data_display" to = "/drone_$(arg drone_id)_planning/data_display"/>
<remap from="~planning/broadcast_bspline_from_planner" to = "/broadcast_bspline"/>
<remap from="~planning/broadcast_bspline_to_planner" to = "/broadcast_bspline"/>
<remap from="~grid_map/odom" to="/drone_$(arg drone_id)_$(arg odometry_topic)"/>
<remap from="~grid_map/cloud" to="/drone_$(arg drone_id)_$(arg cloud_topic)"/>
<remap from="~grid_map/pose" to = "/drone_$(arg drone_id)_$(arg camera_pose_topic)"/>
<remap from="~grid_map/depth" to = "/drone_$(arg drone_id)_$(arg depth_topic)"/>
<!-- planning fsm -->
<param name="fsm/flight_type" value="$(arg flight_type)" type="int"/>
<param name="fsm/thresh_replan_time" value="1.0" type="double"/>
<param name="fsm/thresh_no_replan_meter" value="1.0" type="double"/>
<param name="fsm/planning_horizon" value="$(arg planning_horizon)" type="double"/> <!--always set to 1.5 times grater than sensing horizen-->
<param name="fsm/planning_horizen_time" value="3" type="double"/>
<param name="fsm/emergency_time" value="1.0" type="double"/>
<param name="fsm/realworld_experiment" value="false"/>
<param name="fsm/fail_safe" value="true"/>
<param name="fsm/waypoint_num" value="$(arg point_num)" type="int"/>
<param name="fsm/waypoint0_x" value="$(arg point0_x)" type="double"/>
<param name="fsm/waypoint0_y" value="$(arg point0_y)" type="double"/>
<param name="fsm/waypoint0_z" value="$(arg point0_z)" type="double"/>
<param name="fsm/waypoint1_x" value="$(arg point1_x)" type="double"/>
<param name="fsm/waypoint1_y" value="$(arg point1_y)" type="double"/>
<param name="fsm/waypoint1_z" value="$(arg point1_z)" type="double"/>
<param name="fsm/waypoint2_x" value="$(arg point2_x)" type="double"/>
<param name="fsm/waypoint2_y" value="$(arg point2_y)" type="double"/>
<param name="fsm/waypoint2_z" value="$(arg point2_z)" type="double"/>
<param name="fsm/waypoint3_x" value="$(arg point3_x)" type="double"/>
<param name="fsm/waypoint3_y" value="$(arg point3_y)" type="double"/>
<param name="fsm/waypoint3_z" value="$(arg point3_z)" type="double"/>
<param name="fsm/waypoint4_x" value="$(arg point4_x)" type="double"/>
<param name="fsm/waypoint4_y" value="$(arg point4_y)" type="double"/>
<param name="fsm/waypoint4_z" value="$(arg point4_z)" type="double"/>
<param name="grid_map/resolution" value="0.1" />
<param name="grid_map/map_size_x" value="$(arg map_size_x_)" />
<param name="grid_map/map_size_y" value="$(arg map_size_y_)" />
<param name="grid_map/map_size_z" value="$(arg map_size_z_)" />
<param name="grid_map/local_update_range_x" value="5.5" />
<param name="grid_map/local_update_range_y" value="5.5" />
<param name="grid_map/local_update_range_z" value="4.5" />
<param name="grid_map/obstacles_inflation" value="0.099" />
<param name="grid_map/local_map_margin" value="10"/>
<param name="grid_map/ground_height" value="-0.01"/>
<!-- camera parameter -->
<param name="grid_map/cx" value="$(arg cx)"/>
<param name="grid_map/cy" value="$(arg cy)"/>
<param name="grid_map/fx" value="$(arg fx)"/>
<param name="grid_map/fy" value="$(arg fy)"/>
<!-- depth filter -->
<param name="grid_map/use_depth_filter" value="true"/>
<param name="grid_map/depth_filter_tolerance" value="0.15"/>
<param name="grid_map/depth_filter_maxdist" value="5.0"/>
<param name="grid_map/depth_filter_mindist" value="0.2"/>
<param name="grid_map/depth_filter_margin" value="2"/>
<param name="grid_map/k_depth_scaling_factor" value="1000.0"/>
<param name="grid_map/skip_pixel" value="2"/>
<!-- local fusion -->
<param name="grid_map/p_hit" value="0.65"/>
<param name="grid_map/p_miss" value="0.35"/>
<param name="grid_map/p_min" value="0.12"/>
<param name="grid_map/p_max" value="0.90"/>
<param name="grid_map/p_occ" value="0.80"/>
<param name="grid_map/min_ray_length" value="0.1"/>
<param name="grid_map/max_ray_length" value="4.5"/>
<param name="grid_map/virtual_ceil_height" value="2.9"/>
<param name="grid_map/visualization_truncate_height" value="1.8"/>
<param name="grid_map/show_occ_time" value="false"/>
<param name="grid_map/pose_type" value="1"/>
<param name="grid_map/frame_id" value="world"/>
<!-- planner manager -->
<param name="manager/max_vel" value="$(arg max_vel)" type="double"/>
<param name="manager/max_acc" value="$(arg max_acc)" type="double"/>
<param name="manager/max_jerk" value="4" type="double"/>
<param name="manager/control_points_distance" value="0.4" type="double"/>
<param name="manager/feasibility_tolerance" value="0.05" type="double"/>
<param name="manager/planning_horizon" value="$(arg planning_horizon)" type="double"/>
<param name="manager/use_distinctive_trajs" value="$(arg use_distinctive_trajs)" type="bool"/>
<param name="manager/drone_id" value="$(arg drone_id)"/>
<!-- trajectory optimization -->
<param name="optimization/lambda_smooth" value="1.0" type="double"/>
<param name="optimization/lambda_collision" value="0.5" type="double"/>
<param name="optimization/lambda_feasibility" value="0.1" type="double"/>
<param name="optimization/lambda_fitness" value="1.0" type="double"/>
<param name="optimization/dist0" value="0.5" type="double"/>
<param name="optimization/swarm_clearance" value="0.5" type="double"/>
<param name="optimization/max_vel" value="$(arg max_vel)" type="double"/>
<param name="optimization/max_acc" value="$(arg max_acc)" type="double"/>
<param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/>
<param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/>
<param name="bspline/limit_ratio" value="1.1" type="double"/>
<!-- objects prediction -->
<param name="prediction/obj_num" value="$(arg obj_num_set)" type="int"/>
<param name="prediction/lambda" value="1.0" type="double"/>
<param name="prediction/predict_rate" value="1.0" type="double"/>
</node>
</launch>

3055
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/launch/default.rviz
View File

File diff suppressed because it is too large Load Diff

135
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/launch/run_in_sim.launch
Unescape View File

@ -1,135 +0,0 @@
<launch>
<!-- size of map, change the size inflate x, y, z according to your application -->
<arg name="map_size_x"/>
<arg name="map_size_y"/>
<arg name="map_size_z"/>
<arg name="init_x"/>
<arg name="init_y"/>
<arg name="init_z"/>
<arg name="target_x"/>
<arg name="target_y"/>
<arg name="target_z"/>
<arg name="drone_id"/>
<!-- topic of your odometry such as VIO or LIO -->
<arg name="odom_topic"/>
<!-- number of moving objects -->
<arg name="obj_num" value="10" />
<!-- main algorithm params -->
<include file="$(find ego_planner)/launch/advanced_param.xml">
<arg name="drone_id" value="$(arg drone_id)"/>
<arg name="map_size_x_" value="$(arg map_size_x)"/>
<arg name="map_size_y_" value="$(arg map_size_y)"/>
<arg name="map_size_z_" value="$(arg map_size_z)"/>
<arg name="odometry_topic" value="$(arg odom_topic)"/>
<arg name="obj_num_set" value="$(arg obj_num)" />
<!-- camera pose: transform of camera frame in the world frame -->
<!-- depth topic: depth image, 640x480 by default -->
<!-- don't set cloud_topic if you already set these ones! -->
<arg name="camera_pose_topic" value="pcl_render_node/camera_pose"/>
<arg name="depth_topic" value="pcl_render_node/depth"/>
<!-- topic of point cloud measurement, such as from LIDAR -->
<!-- don't set camera pose and depth, if you already set this one! -->
<arg name="cloud_topic" value="pcl_render_node/cloud"/>
<!-- intrinsic params of the depth camera -->
<arg name="cx" value="321.04638671875"/>
<arg name="cy" value="243.44969177246094"/>
<arg name="fx" value="387.229248046875"/>
<arg name="fy" value="387.229248046875"/>
<!-- maximum velocity and acceleration the drone will reach -->
<arg name="max_vel" value="2.0" />
<arg name="max_acc" value="3.0" />
<!--always set to 1.5 times grater than sensing horizen-->
<arg name="planning_horizon" value="7.5" />
<arg name="use_distinctive_trajs" value="true" />
<!-- 1: use 2D Nav Goal to select goal -->
<!-- 2: use global waypoints below -->
<arg name="flight_type" value="2" />
<!-- global waypoints -->
<!-- It generates a piecewise min-snap traj passing all waypoints -->
<arg name="point_num" value="1" />
<arg name="point0_x" value="$(arg target_x)" />
<arg name="point0_y" value="$(arg target_y)" />
<arg name="point0_z" value="$(arg target_z)" />
<arg name="point1_x" value="0.0" />
<arg name="point1_y" value="15.0" />
<arg name="point1_z" value="1.0" />
<arg name="point2_x" value="15.0" />
<arg name="point2_y" value="0.0" />
<arg name="point2_z" value="1.0" />
<arg name="point3_x" value="0.0" />
<arg name="point3_y" value="-15.0" />
<arg name="point3_z" value="1.0" />
<arg name="point4_x" value="-15.0" />
<arg name="point4_y" value="0.0" />
<arg name="point4_z" value="1.0" />
</include>
<!-- trajectory server -->
<node pkg="ego_planner" name="drone_$(arg drone_id)_traj_server" type="traj_server" output="screen">
<remap from="position_cmd" to="drone_$(arg drone_id)_planning/pos_cmd"/>
<remap from="~planning/bspline" to="drone_$(arg drone_id)_planning/bspline"/>
<param name="traj_server/time_forward" value="1.0" type="double"/>
</node>
<!--node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
<remap from="~odom" to="$(arg odom_topic)"/>
<remap from="~goal" to="/move_base_simple/goal"/>
<remap from="~traj_start_trigger" to="/traj_start_trigger" />
<param name="waypoint_type" value="manual-lonely-waypoint"/>
</node-->
<!-- use simulator -->
<include file="$(find ego_planner)/launch/simulator.xml">
<arg name="drone_id" value="$(arg drone_id)"/>
<arg name="map_size_x_" value="$(arg map_size_x)"/>
<arg name="map_size_y_" value="$(arg map_size_y)"/>
<arg name="map_size_z_" value="$(arg map_size_z)"/>
<arg name="init_x_" value="$(arg init_x)"/>
<arg name="init_y_" value="$(arg init_y)"/>
<arg name="init_z_" value="$(arg init_z)"/>
<arg name="odometry_topic" value="$(arg odom_topic)" />
</include>
<![CDATA[node pkg="plan_env" name="obj_generator" type="obj_generator" output="screen">
<param name="obj_generator/obj_num" value="$(arg obj_num)"/>
<param name="obj_generator/x_size" value="12.0"/>
<param name="obj_generator/y_size" value="12.0"/>
<param name="obj_generator/h_size" value="1.0"/>
<param name="obj_generator/vel" value="1.5"/>
<param name="obj_generator/yaw_dot" value="2.0"/>
<param name="obj_generator/acc_r1" value="1.0"/>
<param name="obj_generator/acc_r2" value="1.0"/>
<param name="obj_generator/acc_z" value="0.0"/>
<param name="obj_generator/scale1" value="0.5"/>
<param name="obj_generator/scale2" value="1.0"/>
<param name="obj_generator/interval" value="100.0"/>
<param name="obj_generator/input_type" value="1"/> <!-- 1: Vel input, 2: Acc input-->>
</node]]>
</launch>

3
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/launch/rviz.launch
Unescape View File

@ -1,3 +0,0 @@
<launch>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ego_planner)/launch/default.rviz" required="true" />
</launch>

4
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/launch/simple_run.launch
Unescape View File

@ -1,4 +0,0 @@
<launch>
<include file="$(find ego_planner)/launch/rviz.launch"/>
<include file="$(find ego_planner)/launch/swarm.launch"/>
</launch>

141
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/launch/simulator.xml
Unescape View File

@ -1,141 +0,0 @@
<launch>
<arg name="init_x_"/>
<arg name="init_y_"/>
<arg name="init_z_"/>
<arg name="obj_num" value="1" />
<arg name="map_size_x_"/>
<arg name="map_size_y_"/>
<arg name="map_size_z_"/>
<arg name="c_num"/>
<arg name="p_num"/>
<arg name="min_dist"/>
<arg name="odometry_topic"/>
<arg name="drone_id"/>
<!-- There are two kinds of maps you can choose, just comment out the one you don’t need like the follow. Have a try. /-->
<![CDATA[node pkg ="map_generator" name ="random_forest" type ="random_forest" output = "screen">
<remap from="~odometry" to="$(arg odometry_topic)"/>
<param name="map/x_size" value="$(arg map_size_x_)" />
<param name="map/y_size" value="$(arg map_size_y_)" />
<param name="map/z_size" value="$(arg map_size_z_)" />
<param name="map/resolution" value="0.1"/>
<param name="ObstacleShape/seed" value="1"/>
<param name="map/obs_num" value="$(arg p_num)"/>
<param name="ObstacleShape/lower_rad" value="0.5"/>
<param name="ObstacleShape/upper_rad" value="0.7"/>
<param name="ObstacleShape/lower_hei" value="0.0"/>
<param name="ObstacleShape/upper_hei" value="3.0"/>
<param name="map/circle_num" value="$(arg c_num)"/>
<param name="ObstacleShape/radius_l" value="0.7"/>
<param name="ObstacleShape/radius_h" value="0.5"/>
<param name="ObstacleShape/z_l" value="0.7"/>
<param name="ObstacleShape/z_h" value="0.8"/>
<param name="ObstacleShape/theta" value="0.5"/>
<param name="sensing/radius" value="5.0"/>
<param name="sensing/rate" value="10.0"/>
<param name="min_distance" value="$(arg min_dist)"/>
</node]]>
<![CDATA[node pkg="mockamap" type="mockamap_node" name="mockamap_node" output="screen">
<remap from="/mock_map" to="/map_generator/global_cloud"/>
<param name="seed" type="int" value="127"/>
<param name="update_freq" type="double" value="0.5"/>
<!-- box edge length, unit meter-->
<param name="resolution" type="double" value="0.1"/>
<!-- map size unit meter-->
<param name="x_length" value="$(arg map_size_x_)"/>
<param name="y_length" value="$(arg map_size_y_)"/>
<param name="z_length" value="$(arg map_size_z_)"/>
<param name="type" type="int" value="1"/>
<!-- 1 perlin noise parameters -->
<!-- complexity: base noise frequency,
large value will be complex
typical 0.0 ~ 0.5 -->
<!-- fill: infill persentage
typical: 0.4 ~ 0.0 -->
<!-- fractal: large value will have more detail-->
<!-- attenuation: for fractal attenuation
typical: 0.0 ~ 0.5 -->
<param name="complexity" type="double" value="0.05"/>
<param name="fill" type="double" value="0.12"/>
<param name="fractal" type="int" value="1"/>
<param name="attenuation" type="double" value="0.1"/>
</node]]>
<!-- <node pkg="so3_quadrotor_simulator" type="quadrotor_simulator_so3" name="drone_$(arg drone_id)_quadrotor_simulator_so3" output="screen">
<param name="rate/odom" value="200.0"/>
<param name="simulator/init_state_x" value="$(arg init_x_)"/>
<param name="simulator/init_state_y" value="$(arg init_y_)"/>
<param name="simulator/init_state_z" value="$(arg init_z_)"/>
<remap from="~odom" to="drone_$(arg drone_id)_visual_slam/odom"/>
<remap from="~cmd" to="drone_$(arg drone_id)_so3_cmd"/>
<remap from="~force_disturbance" to="drone_$(arg drone_id)_force_disturbance"/>
<remap from="~moment_disturbance" to="drone_$(arg drone_id)_moment_disturbance"/>
</node>
<node pkg="nodelet" type="nodelet" args="standalone so3_control/SO3ControlNodelet" name="drone_$(arg drone_id)_so3_control" required="true" output="screen">
<param name="so3_control/init_state_x" value="$(arg init_x_)"/>
<param name="so3_control/init_state_y" value="$(arg init_y_)"/>
<param name="so3_control/init_state_z" value="$(arg init_z_)"/>
<remap from="~odom" to="drone_$(arg drone_id)_visual_slam/odom"/>
<remap from="~position_cmd" to="drone_$(arg drone_id)_planning/pos_cmd"/>
<remap from="~motors" to="drone_$(arg drone_id)_motors"/>
<remap from="~corrections" to="drone_$(arg drone_id)_corrections"/>
<remap from="~so3_cmd" to="drone_$(arg drone_id)_so3_cmd"/>
<rosparam file="$(find so3_control)/config/gains_hummingbird.yaml"/>
<rosparam file="$(find so3_control)/config/corrections_hummingbird.yaml"/>
<param name="mass" value="0.98"/>
<param name="use_angle_corrections " value="false"/>
<param name="use_external_yaw " value="false"/>
<param name="gains/rot/z" value="1.0"/>
<param name="gains/ang/z" value="0.1"/>
</node> -->
<node pkg="poscmd_2_odom" name="drone_$(arg drone_id)_poscmd_2_odom" type="poscmd_2_odom" output="screen">
<param name="init_x" value="$(arg init_x_)"/>
<param name="init_y" value="$(arg init_y_)"/>
<param name="init_z" value="$(arg init_z_)"/>
<remap from="~command" to="drone_$(arg drone_id)_planning/pos_cmd"/>
<remap from="~odometry" to="drone_$(arg drone_id)_$(arg odometry_topic)"/>
</node>
<node pkg="odom_visualization" name="drone_$(arg drone_id)_odom_visualization" type="odom_visualization" output="screen">
<remap from="~odom" to="drone_$(arg drone_id)_visual_slam/odom"/>
<param name="color/a" value="1.0"/>
<param name="color/r" value="0.0"/>
<param name="color/g" value="0.0"/>
<param name="color/b" value="0.0"/>
<param name="covariance_scale" value="100.0"/>
<param name="robot_scale" value="1.0"/>
<param name="tf45" value="false"/>
<param name="drone_id" value="drone_id"/>
</node>
<node pkg="local_sensing_node" type="pcl_render_node" name="drone_$(arg drone_id)_pcl_render_node" output="screen">
<rosparam command="load" file="$(find local_sensing_node)/params/camera.yaml" />
<param name="sensing_horizon" value="5.0" />
<param name="sensing_rate" value="30.0"/>
<param name="estimation_rate" value="30.0"/>
<param name="map/x_size" value="$(arg map_size_x_)"/>
<param name="map/y_size" value="$(arg map_size_y_)"/>
<param name="map/z_size" value="$(arg map_size_z_)"/>
<remap from="~global_map" to="/map_generator/global_cloud"/>
<remap from="~odometry" to="/drone_$(arg drone_id)_$(arg odometry_topic)"/>
<remap from="~pcl_render_node/cloud" to="/drone_$(arg drone_id)_pcl_render_node/cloud"/>
</node>
</launch>

117
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/launch/single_run_in_sim.launch
Unescape View File

@ -1,117 +0,0 @@
<launch>
<!-- number of moving objects -->
<arg name="obj_num" value="10" />
<arg name="drone_id" value="0"/>
<arg name="map_size_x" value="50.0"/>
<arg name="map_size_y" value="25.0"/>
<arg name="map_size_z" value=" 2.0"/>
<arg name="odom_topic" value="visual_slam/odom" />
<!-- map -->
<!-- <node pkg ="map_generator" name ="random_forest" type ="random_forest" output = "screen">
<param name="map/x_size" value="26" />
<param name="map/y_size" value="20" />
<param name="map/z_size" value="3" />
<param name="map/resolution" value="0.1"/>
<param name="ObstacleShape/seed" value="1"/>
<param name="map/obs_num" value="250"/>
<param name="ObstacleShape/lower_rad" value="0.5"/>
<param name="ObstacleShape/upper_rad" value="0.7"/>
<param name="ObstacleShape/lower_hei" value="0.0"/>
<param name="ObstacleShape/upper_hei" value="3.0"/>
<param name="map/circle_num" value="250"/>
<param name="ObstacleShape/radius_l" value="0.7"/>
<param name="ObstacleShape/radius_h" value="0.5"/>
<param name="ObstacleShape/z_l" value="0.7"/>
<param name="ObstacleShape/z_h" value="0.8"/>
<param name="ObstacleShape/theta" value="0.5"/>
<param name="pub_rate" value="1.0"/>
<param name="min_distance" value="0.8"/>
</node> -->
<!-- map generator new-->
<node pkg ="map_generator" name ="random_forest_cxy" type ="random_forest_cxy" output = "screen">
<param name="map_generator/x_size" value="$(arg map_size_x)" />
<param name="map_generator/y_size" value="$(arg map_size_y)" />
<param name="map_generator/z_size" value="$(arg map_size_z)" />
<param name="map_generator/resolution" value="0.1"/>
<param name="map_generator/cylinder_num" value="40"/>
<param name="map_generator/sqaure_num" value="40"/>
<param name="map_generator/cylinder_radius" value="0.2"/>
<param name="map_generator/cylinder_height" value="3.0"/>
<param name="map_generator/sqaure_size" value="0.4"/>
<param name="map_generator/sensing_range" value="10.0"/>
<param name="map_generator/sense_rate" value="10.0"/>
<param name="map_generator/min_distance" value="1.5"/>
</node>
<!-- main algorithm params -->
<include file="$(find ego_planner)/launch/advanced_param.xml">
<arg name="drone_id" value="$(arg drone_id)"/>
<arg name="map_size_x_" value="$(arg map_size_x)"/>
<arg name="map_size_y_" value="$(arg map_size_y)"/>
<arg name="map_size_z_" value="$(arg map_size_z)"/>
<arg name="odometry_topic" value="$(arg odom_topic)"/>
<arg name="obj_num_set" value="$(arg obj_num)" />
<!-- camera pose: transform of camera frame in the world frame -->
<!-- depth topic: depth image, 640x480 by default -->
<!-- don't set cloud_topic if you already set these ones! -->
<arg name="camera_pose_topic" value="pcl_render_node/camera_pose"/>
<arg name="depth_topic" value="pcl_render_node/depth"/>
<!-- topic of point cloud measurement, such as from LIDAR -->
<!-- don't set camera pose and depth, if you already set this one! -->
<arg name="cloud_topic" value="pcl_render_node/cloud"/>
<!-- intrinsic params of the depth camera -->
<arg name="cx" value="321.04638671875"/>
<arg name="cy" value="243.44969177246094"/>
<arg name="fx" value="387.229248046875"/>
<arg name="fy" value="387.229248046875"/>
<!-- maximum velocity and acceleration the drone will reach -->
<arg name="max_vel" value="2.0" />
<arg name="max_acc" value="6.0" />
<!--always set to 1.5 times grater than sensing horizen-->
<arg name="planning_horizon" value="7.5" />
<arg name="use_distinctive_trajs" value="true" />
<!-- 1: use 2D Nav Goal to select goal -->
<!-- 2: use global waypoints below -->
<arg name="flight_type" value="2" />
<!-- global waypoints -->
<!-- It generates a piecewise min-snap traj passing all waypoints -->
<arg name="point_num" value="4" />
<arg name="point0_x" value="15" />
<arg name="point0_y" value="0" />
<arg name="point0_z" value="1" />
<arg name="point1_x" value="-15.0" />
<arg name="point1_y" value="0.0" />
<arg name="point1_z" value="1.0" />
<arg name="point2_x" value="15.0" />
<arg name="point2_y" value="0.0" />
<arg name="point2_z" value="1.0" />
<arg name="point3_x" value="-15.0" />
<arg name="point3_y" value="0.0" />
<arg name="point3_z" value="1.0" />
<arg name="point4_x" value="15.0" />
<arg name="point4_y" value="0.0" />
<arg name="point4_z" value="1.0" />
</include>
<!-- trajectory server -->
<node pkg="ego_planner" name="drone_$(arg drone_id)_traj_server" type="traj_server" output="screen">
<remap from="position_cmd" to="drone_$(arg drone_id)_planning/pos_cmd"/>
<remap from="~planning/bspline" to="drone_$(arg drone_id)_planning/bspline"/>
<param name="traj_server/time_forward" value="1.0" type="double"/>
</node>
<!-- use simulator -->
<include file="$(find ego_planner)/launch/simulator.xml">
<arg name="drone_id" value="$(arg drone_id)"/>
<arg name="map_size_x_" value="$(arg map_size_x)"/>
<arg name="map_size_y_" value="$(arg map_size_y)"/>
<arg name="map_size_z_" value="$(arg map_size_z)"/>
<arg name="init_x_" value="-15"/>
<arg name="init_y_" value="0"/>
<arg name="init_z_" value="0.1"/>
<arg name="odometry_topic" value="$(arg odom_topic)" />
</include>
</launch>

205
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/launch/swarm.launch
Unescape View File

@ -1,205 +0,0 @@
<launch>
<arg name="map_size_x" value="42.0"/>
<arg name="map_size_y" value="30.0"/>
<arg name="map_size_z" value=" 5.0"/>
<arg name="odom_topic" value="visual_slam/odom" />
<node pkg ="map_generator" name ="random_forest" type ="random_forest" output = "screen">
<param name="map/x_size" value="36" />
<param name="map/y_size" value="20" />
<param name="map/z_size" value="3" />
<param name="map/resolution" value="0.1"/>
<param name="ObstacleShape/seed" value="1"/>
<param name="map/obs_num" value="200"/>
<param name="ObstacleShape/lower_rad" value="0.5"/>
<param name="ObstacleShape/upper_rad" value="0.7"/>
<param name="ObstacleShape/lower_hei" value="0.0"/>
<param name="ObstacleShape/upper_hei" value="3.0"/>
<param name="map/circle_num" value="200"/>
<param name="ObstacleShape/radius_l" value="0.7"/>
<param name="ObstacleShape/radius_h" value="0.5"/>
<param name="ObstacleShape/z_l" value="0.7"/>
<param name="ObstacleShape/z_h" value="0.8"/>
<param name="ObstacleShape/theta" value="0.5"/>
<param name="sensing/radius" value="5.0"/>
<param name="sensing/rate" value="1.0"/>
<param name="min_distance" value="1.2"/>
</node>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="0"/>
<arg name="init_x" value="-20.0"/>
<arg name="init_y" value="-9.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="20.0"/>
<arg name="target_y" value="9.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="1"/>
<arg name="init_x" value="-20.0"/>
<arg name="init_y" value="-7.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="20.0"/>
<arg name="target_y" value="7.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="2"/>
<arg name="init_x" value="-20.0"/>
<arg name="init_y" value="-5.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="20.0"/>
<arg name="target_y" value="5.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="3"/>
<arg name="init_x" value="-20.0"/>
<arg name="init_y" value="-3.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="20.0"/>
<arg name="target_y" value="3.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="4"/>
<arg name="init_x" value="-20.0"/>
<arg name="init_y" value="-1.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="20.0"/>
<arg name="target_y" value="1.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="5"/>
<arg name="init_x" value="-20.0"/>
<arg name="init_y" value="1.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="20.0"/>
<arg name="target_y" value="-1.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="6"/>
<arg name="init_x" value="-20.0"/>
<arg name="init_y" value="3.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="20.0"/>
<arg name="target_y" value="-3.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="7"/>
<arg name="init_x" value="-20.0"/>
<arg name="init_y" value="5.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="20.0"/>
<arg name="target_y" value="-5.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="8"/>
<arg name="init_x" value="-20.0"/>
<arg name="init_y" value="7.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="20.0"/>
<arg name="target_y" value="-7.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="9"/>
<arg name="init_x" value="-20.0"/>
<arg name="init_y" value="9.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="20.0"/>
<arg name="target_y" value="-9.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
</launch>

412
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/launch/swarm_large.launch
Unescape View File

@ -1,412 +0,0 @@
<launch>
<arg name="map_size_x" value="72.0"/>
<arg name="map_size_y" value="30.0"/>
<arg name="map_size_z" value=" 5.0"/>
<arg name="odom_topic" value="visual_slam/odom" />
<node pkg ="map_generator" name ="random_forest" type ="random_forest" output = "screen">
<param name="map/x_size" value="66" />
<param name="map/y_size" value="30" />
<param name="map/z_size" value="3" />
<param name="map/resolution" value="0.1"/>
<param name="ObstacleShape/seed" value="1"/>
<param name="map/obs_num" value="300"/>
<param name="ObstacleShape/lower_rad" value="0.5"/>
<param name="ObstacleShape/upper_rad" value="0.7"/>
<param name="ObstacleShape/lower_hei" value="0.0"/>
<param name="ObstacleShape/upper_hei" value="3.0"/>
<param name="map/circle_num" value="200"/>
<param name="ObstacleShape/radius_l" value="0.7"/>
<param name="ObstacleShape/radius_h" value="0.5"/>
<param name="ObstacleShape/z_l" value="0.7"/>
<param name="ObstacleShape/z_h" value="0.8"/>
<param name="ObstacleShape/theta" value="0.5"/>
<param name="sensing/radius" value="5.0"/>
<param name="sensing/rate" value="1.0"/>
<param name="min_distance" value="1.2"/>
</node>
<!-- <node pkg="mockamap" type="mockamap_node" name="mockamap_node" output="screen">
<remap from="/mock_map" to="/map_generator/global_cloud"/>
<param name="seed" type="int" value="127"/>
<param name="update_freq" type="double" value="0.5"/>
<param name="resolution" type="double" value="0.1"/>
<param name="x_length" value="66"/>
<param name="y_length" value="30"/>
<param name="z_length" value="3"/>
<param name="type" type="int" value="1"/>
<param name="complexity" type="double" value="0.05"/>
<param name="fill" type="double" value="0.12"/>
<param name="fractal" type="int" value="1"/>
<param name="attenuation" type="double" value="0.1"/>
</node> -->
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="0"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="-10.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="-10.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="1"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="-9.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="-9.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="2"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="-8.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="-8.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="3"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="-7.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="-7.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="4"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="-6.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="-6.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="5"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="-5.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="-5.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="6"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="-4.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="-4.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="7"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="-3.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="-3.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="8"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="-2.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="-2.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="9"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="-1.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="-1.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="10"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="0.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="0.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="11"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="1.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="1.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="12"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="2.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="2.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="13"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="3.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="3.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="14"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="4.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="4.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="15"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="5.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="5.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="16"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="6.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="6.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="17"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="7.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="7.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="18"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="8.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="8.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="19"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="9.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="9.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
<include file="$(find ego_planner)/launch/run_in_sim.launch">
<arg name="drone_id" value="20"/>
<arg name="init_x" value="-35.0"/>
<arg name="init_y" value="10.0"/>
<arg name="init_z" value="0.1"/>
<arg name="target_x" value="35.0"/>
<arg name="target_y" value="10.0"/>
<arg name="target_z" value="1"/>
<arg name="map_size_x" value="$(arg map_size_x)"/>
<arg name="map_size_y" value="$(arg map_size_y)"/>
<arg name="map_size_z" value="$(arg map_size_z)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>
</launch>

82
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/package.xml
Unescape View File

@ -1,82 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>ego_planner</name>
<version>0.0.0</version>
<description>The ego_planner package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ego_planner</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>plan_env</build_depend>
<build_depend>path_searching</build_depend>
<build_depend>bspline_opt</build_depend>
<build_depend>traj_utils</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>plan_env</exec_depend>
<exec_depend>path_searching</exec_depend>
<exec_depend>bspline_opt</exec_depend>
<exec_depend>traj_utils</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
<!-- catkin_make -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=Yes -->

56
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/src/ego_planner_node.cpp
Unescape View File

@ -1,56 +0,0 @@
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <plan_manage/ego_replan_fsm.h>
using namespace ego_planner;
int main(int argc, char **argv)
{
ros::init(argc, argv, "ego_planner_node");
ros::NodeHandle nh("~");
EGOReplanFSM rebo_replan;
rebo_replan.init(nh);
// ros::Duration(1.0).sleep();
ros::spin();
return 0;
}
// #include <ros/ros.h>
// #include <csignal>
// #include <visualization_msgs/Marker.h>
// #include <plan_manage/ego_replan_fsm.h>
// using namespace ego_planner;
// void SignalHandler(int signal) {
// if(ros::isInitialized() && ros::isStarted() && ros::ok() && !ros::isShuttingDown()){
// ros::shutdown();
// }
// }
// int main(int argc, char **argv) {
// signal(SIGINT, SignalHandler);
// signal(SIGTERM,SignalHandler);
// ros::init(argc, argv, "ego_planner_node", ros::init_options::NoSigintHandler);
// ros::NodeHandle nh("~");
// EGOReplanFSM rebo_replan;
// rebo_replan.init(nh);
// // ros::Duration(1.0).sleep();
// ros::AsyncSpinner async_spinner(4);
// async_spinner.start();
// ros::waitForShutdown();
// return 0;
// }

954
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/src/ego_replan_fsm.cpp
Unescape View File

@ -1,954 +0,0 @@
#include <plan_manage/ego_replan_fsm.h>
namespace ego_planner
{
void EGOReplanFSM::init(ros::NodeHandle &nh)
{
current_wp_ = 0;
exec_state_ = FSM_EXEC_STATE::INIT;
have_target_ = false;
have_odom_ = false;
have_recv_pre_agent_ = false;
/* fsm param */
// 目标点类型：1，手动设定目标点；2，预设目标点
nh.param("fsm/flight_type", target_type_, -1);
// 重规划时间间隔
nh.param("fsm/thresh_replan_time", replan_thresh_, -1.0);
// 与目标距离小于该参数时，停止规划
nh.param("fsm/thresh_no_replan_meter", no_replan_thresh_, -1.0);
// 规划范围
nh.param("fsm/planning_horizon", planning_horizen_, -1.0);
// 紧急停止时间
nh.param("fsm/emergency_time", emergency_time_, 1.0);
// 仿真与实际标志位，实际中需要等待一个起始话题
nh.param("fsm/realworld_experiment", flag_realworld_experiment_, false);
// 未知
nh.param("fsm/fail_safe", enable_fail_safe_, true);
// 真实实验中需要等待"/traj_start_trigger"话题
have_trigger_ = !flag_realworld_experiment_;
// 读取waypoint
nh.param("fsm/waypoint_num", waypoint_num_, -1);
for (int i = 0; i < waypoint_num_; i++)
{
nh.param("fsm/waypoint" + to_string(i) + "_x", waypoints_[i][0], -1.0);
nh.param("fsm/waypoint" + to_string(i) + "_y", waypoints_[i][1], -1.0);
nh.param("fsm/waypoint" + to_string(i) + "_z", waypoints_[i][2], -1.0);
}
/* initialize main modules */
// 显示类
visualization_.reset(new PlanningVisualization(nh));
// 规划器类
planner_manager_.reset(new EGOPlannerManager);
planner_manager_->initPlanModules(nh, visualization_);
planner_manager_->deliverTrajToOptimizer(); // store trajectories
planner_manager_->setDroneIdtoOpt();
/* callback */
// 规划状态机定时器
exec_timer_ = nh.createTimer(ros::Duration(0.01), &EGOReplanFSM::execFSMCallback, this);
// 安全检查定时器
safety_timer_ = nh.createTimer(ros::Duration(0.05), &EGOReplanFSM::checkCollisionCallback, this);
// 订阅里程计
odom_sub_ = nh.subscribe("odom_world", 1, &EGOReplanFSM::odometryCallback, this);
// 订阅其他无人机位置
// ego默认从0开始，我们默认从1开始，因此这里>2
if (planner_manager_->pp_.drone_id >= 2)
{
string sub_topic_name = string("/uav") + std::to_string(planner_manager_->pp_.drone_id - 1) + string("_planning/swarm_trajs");
swarm_trajs_sub_ = nh.subscribe(sub_topic_name.c_str(), 10, &EGOReplanFSM::swarmTrajsCallback, this, ros::TransportHints().tcpNoDelay());
}
string pub_topic_name = string("/uav") + std::to_string(planner_manager_->pp_.drone_id) + string("_planning/swarm_trajs");
swarm_trajs_pub_ = nh.advertise<traj_utils::MultiBsplines>(pub_topic_name.c_str(), 10);
broadcast_bspline_pub_ = nh.advertise<traj_utils::Bspline>("planning/broadcast_bspline_from_planner", 10);
broadcast_bspline_sub_ = nh.subscribe("planning/broadcast_bspline_to_planner", 100, &EGOReplanFSM::BroadcastBsplineCallback, this, ros::TransportHints().tcpNoDelay());
bspline_pub_ = nh.advertise<traj_utils::Bspline>("planning/bspline", 10);
data_disp_pub_ = nh.advertise<traj_utils::DataDisp>("planning/data_display", 100);
if (target_type_ == TARGET_TYPE::MANUAL_TARGET)
{
string waypoint_topic_name = string("/uav") + std::to_string(planner_manager_->pp_.drone_id) + string("/prometheus/ego/goal");
waypoint_sub_ = nh.subscribe(waypoint_topic_name.c_str(), 1, &EGOReplanFSM::waypointCallback, this);
}
else if (target_type_ == TARGET_TYPE::PRESET_TARGET)
{
trigger_sub_ = nh.subscribe("/traj_start_trigger", 1, &EGOReplanFSM::triggerCallback, this);
ROS_INFO("Wait for 1 second.");
int count = 0;
while (ros::ok() && count++ < 1000)
{
ros::spinOnce();
ros::Duration(0.001).sleep();
}
ROS_WARN("Waiting for trigger from [n3ctrl] from RC");
while (ros::ok() && (!have_odom_ || !have_trigger_))
{
ros::spinOnce();
ros::Duration(0.001).sleep();
}
readGivenWps();
}
else
cout << "Wrong target_type_ value! target_type_=" << target_type_ << endl;
}
void EGOReplanFSM::readGivenWps()
{
if (waypoint_num_ <= 0)
{
ROS_ERROR("Wrong waypoint_num_ = %d", waypoint_num_);
return;
}
wps_.resize(waypoint_num_);
for (int i = 0; i < waypoint_num_; i++)
{
wps_[i](0) = waypoints_[i][0];
wps_[i](1) = waypoints_[i][1];
wps_[i](2) = waypoints_[i][2];
}
for (size_t i = 0; i < (size_t)waypoint_num_; i++)
{
// 发布目标点用于显示 "/drone_x_ego_planner_node/goal_point" - [目标点,颜色,大小,id]
visualization_->displayGoalPoint(wps_[i], Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, i);
ros::Duration(0.001).sleep();
}
// 执行第一个路径点
wp_id_ = 0;
planNextWaypoint(wps_[wp_id_]);
}
void EGOReplanFSM::planNextWaypoint(const Eigen::Vector3d next_wp)
{
bool success = false;
// planGlobalTraj(起始位置\速度\加速度,终点位置\速度\加速度)
success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), next_wp, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
if (success)
{
end_pt_ = next_wp;
constexpr double step_size_t = 0.1;
// planner_manager_->global_data_.global_duration_是整段轨迹的预估时间
int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t);
vector<Eigen::Vector3d> gloabl_traj(i_end);
for (int i = 0; i < i_end; i++)
{
// 按照step_size_t提取路径点,按照时间分布
gloabl_traj[i] = planner_manager_->global_data_.global_traj_.evaluate(i * step_size_t);
}
end_vel_.setZero();
have_target_ = true;
have_new_target_ = true;
/*** FSM ***/
if (exec_state_ == WAIT_TARGET)
changeFSMExecState(GEN_NEW_TRAJ, "TRIG");
else
{
while (exec_state_ != EXEC_TRAJ)
{
ros::spinOnce();
ros::Duration(0.001).sleep();
}
changeFSMExecState(REPLAN_TRAJ, "TRIG");
}
// 发布GlobalPath用于显示 "/drone_x_ego_planner_node/global_list" - [GlobalPath,大小,id]
visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);
}
else
{
ROS_ERROR("Unable to generate global trajectory!");
}
}
void EGOReplanFSM::triggerCallback(const geometry_msgs::PoseStampedPtr &msg)
{
have_trigger_ = true;
cout << "Triggered!" << endl;
init_pt_ = odom_pos_;
}
void EGOReplanFSM::waypointCallback(const geometry_msgs::PoseStampedPtr &msg)
{
if (msg->pose.position.z < -0.1)
return;
if(msg->pose.position.x == 99.99 && msg->pose.position.y == 99.99)
{
callEmergencyStop(odom_pos_);
have_target_ = false;
exec_state_ = WAIT_TARGET;
return;
}
callEmergencyStop(odom_pos_);
sleep(2.0);
cout << "Get goal!" << endl;
init_pt_ = odom_pos_;
// 此处定高1米
Eigen::Vector3d end_wp(msg->pose.position.x, msg->pose.position.y, 1.0);
// 发布目标点用于显示 - [目标点,颜色,大小,id]
visualization_->displayGoalPoint(end_wp, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 1);
planNextWaypoint(end_wp);
}
void EGOReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr &msg)
{
odom_pos_(0) = msg->pose.pose.position.x;
odom_pos_(1) = msg->pose.pose.position.y;
odom_pos_(2) = msg->pose.pose.position.z;
odom_vel_(0) = msg->twist.twist.linear.x;
odom_vel_(1) = msg->twist.twist.linear.y;
odom_vel_(2) = msg->twist.twist.linear.z;
//odom_acc_ = estimateAcc( msg );
odom_orient_.w() = msg->pose.pose.orientation.w;
odom_orient_.x() = msg->pose.pose.orientation.x;
odom_orient_.y() = msg->pose.pose.orientation.y;
odom_orient_.z() = msg->pose.pose.orientation.z;
have_odom_ = true;
}
void EGOReplanFSM::BroadcastBsplineCallback(const traj_utils::BsplinePtr &msg)
{
printf("BroadcastBsplineCallback!\n");
size_t id = msg->drone_id;
if ((int)id == planner_manager_->pp_.drone_id)
return;
if (abs((ros::Time::now() - msg->start_time).toSec()) > 0.25)
{
ROS_ERROR("Time difference is too large! Local - Remote Agent %d = %fs",
msg->drone_id, (ros::Time::now() - msg->start_time).toSec());
return;
}
/* Fill up the buffer */
if (planner_manager_->swarm_trajs_buf_.size() <= id)
{
for (size_t i = planner_manager_->swarm_trajs_buf_.size(); i <= id; i++)
{
OneTrajDataOfSwarm blank;
blank.drone_id = -1;
planner_manager_->swarm_trajs_buf_.push_back(blank);
}
}
/* Test distance to the agent */
Eigen::Vector3d cp0(msg->pos_pts[0].x, msg->pos_pts[0].y, msg->pos_pts[0].z);
Eigen::Vector3d cp1(msg->pos_pts[1].x, msg->pos_pts[1].y, msg->pos_pts[1].z);
Eigen::Vector3d cp2(msg->pos_pts[2].x, msg->pos_pts[2].y, msg->pos_pts[2].z);
Eigen::Vector3d swarm_start_pt = (cp0 + 4 * cp1 + cp2) / 6;
if ((swarm_start_pt - odom_pos_).norm() > planning_horizen_ * 4.0f / 3.0f)
{
planner_manager_->swarm_trajs_buf_[id].drone_id = -1;
return; // if the current drone is too far to the received agent.
}
/* Store data */
Eigen::MatrixXd pos_pts(3, msg->pos_pts.size());
Eigen::VectorXd knots(msg->knots.size());
for (size_t j = 0; j < msg->knots.size(); ++j)
{
knots(j) = msg->knots[j];
}
for (size_t j = 0; j < msg->pos_pts.size(); ++j)
{
pos_pts(0, j) = msg->pos_pts[j].x;
pos_pts(1, j) = msg->pos_pts[j].y;
pos_pts(2, j) = msg->pos_pts[j].z;
}
planner_manager_->swarm_trajs_buf_[id].drone_id = id;
if (msg->order % 2)
{
double cutback = (double)msg->order / 2 + 1.5;
planner_manager_->swarm_trajs_buf_[id].duration_ = msg->knots[msg->knots.size() - ceil(cutback)];
}
else
{
double cutback = (double)msg->order / 2 + 1.5;
planner_manager_->swarm_trajs_buf_[id].duration_ = (msg->knots[msg->knots.size() - floor(cutback)] + msg->knots[msg->knots.size() - ceil(cutback)]) / 2;
}
UniformBspline pos_traj(pos_pts, msg->order, msg->knots[1] - msg->knots[0]);
pos_traj.setKnot(knots);
planner_manager_->swarm_trajs_buf_[id].position_traj_ = pos_traj;
planner_manager_->swarm_trajs_buf_[id].start_pos_ = planner_manager_->swarm_trajs_buf_[id].position_traj_.evaluateDeBoorT(0);
planner_manager_->swarm_trajs_buf_[id].start_time_ = msg->start_time;
// planner_manager_->swarm_trajs_buf_[id].start_time_ = ros::Time::now(); // Un-reliable time sync
/* Check Collision */
printf("Check Collision\n");
if (planner_manager_->checkCollision(id))
{
changeFSMExecState(REPLAN_TRAJ, "TRAJ_CHECK");
}
}
void EGOReplanFSM::swarmTrajsCallback(const traj_utils::MultiBsplinesPtr &msg)
{
multi_bspline_msgs_buf_.traj.clear();
multi_bspline_msgs_buf_ = *msg;
// cout << "\033[45;33mmulti_bspline_msgs_buf.drone_id_from=" << multi_bspline_msgs_buf_.drone_id_from << " multi_bspline_msgs_buf_.traj.size()=" << multi_bspline_msgs_buf_.traj.size() << "\033[0m" << endl;
if (!have_odom_)
{
ROS_ERROR("swarmTrajsCallback(): no odom!, return.");
return;
}
if ((int)msg->traj.size() != msg->drone_id_from + 1) // drone_id must start from 0
{
ROS_ERROR("Wrong trajectory size! msg->traj.size()=%d, msg->drone_id_from+1=%d", (int)msg->traj.size(), msg->drone_id_from + 1);
return;
}
if (msg->traj[0].order != 3) // only support B-spline order equals 3.
{
ROS_ERROR("Only support B-spline order equals 3.");
return;
}
// Step 1. receive the trajectories
planner_manager_->swarm_trajs_buf_.clear();
planner_manager_->swarm_trajs_buf_.resize(msg->traj.size());
for (size_t i = 0; i < msg->traj.size(); i++)
{
Eigen::Vector3d cp0(msg->traj[i].pos_pts[0].x, msg->traj[i].pos_pts[0].y, msg->traj[i].pos_pts[0].z);
Eigen::Vector3d cp1(msg->traj[i].pos_pts[1].x, msg->traj[i].pos_pts[1].y, msg->traj[i].pos_pts[1].z);
Eigen::Vector3d cp2(msg->traj[i].pos_pts[2].x, msg->traj[i].pos_pts[2].y, msg->traj[i].pos_pts[2].z);
Eigen::Vector3d swarm_start_pt = (cp0 + 4 * cp1 + cp2) / 6;
if ((swarm_start_pt - odom_pos_).norm() > planning_horizen_ * 4.0f / 3.0f)
{
planner_manager_->swarm_trajs_buf_[i].drone_id = -1;
continue;
}
Eigen::MatrixXd pos_pts(3, msg->traj[i].pos_pts.size());
Eigen::VectorXd knots(msg->traj[i].knots.size());
for (size_t j = 0; j < msg->traj[i].knots.size(); ++j)
{
knots(j) = msg->traj[i].knots[j];
}
for (size_t j = 0; j < msg->traj[i].pos_pts.size(); ++j)
{
pos_pts(0, j) = msg->traj[i].pos_pts[j].x;
pos_pts(1, j) = msg->traj[i].pos_pts[j].y;
pos_pts(2, j) = msg->traj[i].pos_pts[j].z;
}
planner_manager_->swarm_trajs_buf_[i].drone_id = i;
if (msg->traj[i].order % 2)
{
double cutback = (double)msg->traj[i].order / 2 + 1.5;
planner_manager_->swarm_trajs_buf_[i].duration_ = msg->traj[i].knots[msg->traj[i].knots.size() - ceil(cutback)];
}
else
{
double cutback = (double)msg->traj[i].order / 2 + 1.5;
planner_manager_->swarm_trajs_buf_[i].duration_ = (msg->traj[i].knots[msg->traj[i].knots.size() - floor(cutback)] + msg->traj[i].knots[msg->traj[i].knots.size() - ceil(cutback)]) / 2;
}
// planner_manager_->swarm_trajs_buf_[i].position_traj_ =
UniformBspline pos_traj(pos_pts, msg->traj[i].order, msg->traj[i].knots[1] - msg->traj[i].knots[0]);
pos_traj.setKnot(knots);
planner_manager_->swarm_trajs_buf_[i].position_traj_ = pos_traj;
planner_manager_->swarm_trajs_buf_[i].start_pos_ = planner_manager_->swarm_trajs_buf_[i].position_traj_.evaluateDeBoorT(0);
planner_manager_->swarm_trajs_buf_[i].start_time_ = msg->traj[i].start_time;
}
have_recv_pre_agent_ = true;
printf("have_recv_pre_agent_==true\n");
}
void EGOReplanFSM::changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call)
{
if (new_state == exec_state_)
continously_called_times_++;
else
continously_called_times_ = 1;
static string state_str[8] = {"INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP", "SEQUENTIAL_START"};
int pre_s = int(exec_state_);
exec_state_ = new_state;
// COMMENT
cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl;
}
std::pair<int, EGOReplanFSM::FSM_EXEC_STATE> EGOReplanFSM::timesOfConsecutiveStateCalls()
{
return std::pair<int, FSM_EXEC_STATE>(continously_called_times_, exec_state_);
}
void EGOReplanFSM::printFSMExecState()
{
static string state_str[8] = {"INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP", "SEQUENTIAL_START"};
cout << "/uav"<< planner_manager_->pp_.drone_id <<" planner state: " + state_str[int(exec_state_)] << endl;
}
void EGOReplanFSM::execFSMCallback(const ros::TimerEvent &e)
{
// 暂停计时
exec_timer_.stop(); // To avoid blockage (阻塞)
static int fsm_num = 0;
fsm_num++;
if (fsm_num == 1000)
{
printFSMExecState();
if (!have_odom_)
cout << "no odom." << endl;
if (!have_target_)
// cout << "wait for goal or trigger." << endl;
fsm_num = 0;
}
switch (exec_state_)
{
case INIT:
{
if (!have_odom_)
{
goto force_return;
// return;
}
changeFSMExecState(WAIT_TARGET, "FSM");
break;
}
case WAIT_TARGET:
{
if (!have_target_ || !have_trigger_)
goto force_return;
// return;
else
{
changeFSMExecState(SEQUENTIAL_START, "FSM");
}
break;
}
// 按顺序启动
case SEQUENTIAL_START: // for swarm
{
// swarmTrajsCallback回调后,have_recv_pre_agent_会被设置为true
// ego默认从0开始，我们默认从1开始，因此这里>2
if (planner_manager_->pp_.drone_id <= 1 || (planner_manager_->pp_.drone_id >= 2&& have_recv_pre_agent_))
{
if (have_odom_ && have_target_ && have_trigger_)
{
bool success = planFromGlobalTraj(10); // zx-todo
if (success)
{
changeFSMExecState(EXEC_TRAJ, "FSM");
publishSwarmTrajs(true);
}
else
{
ROS_ERROR("Failed to generate the first trajectory!!!");
changeFSMExecState(SEQUENTIAL_START, "FSM");
}
}
else
{
ROS_ERROR("No odom or no target! have_odom_=%d, have_target_=%d", have_odom_, have_target_);
}
}
break;
}
case GEN_NEW_TRAJ:
{
// Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1);
// start_yaw_(0) = atan2(rot_x(1), rot_x(0));
// start_yaw_(1) = start_yaw_(2) = 0.0;
bool success = planFromGlobalTraj(10); // zx-todo
if (success)
{
changeFSMExecState(EXEC_TRAJ, "FSM");
flag_escape_emergency_ = true;
publishSwarmTrajs(false);
}
else
{
changeFSMExecState(GEN_NEW_TRAJ, "FSM");
}
break;
}
case REPLAN_TRAJ:
{
if (planFromCurrentTraj(1))
{
changeFSMExecState(EXEC_TRAJ, "FSM");
publishSwarmTrajs(false);
}
else
{
changeFSMExecState(REPLAN_TRAJ, "FSM");
}
break;
}
case EXEC_TRAJ:
{
/* determine if need to replan */
LocalTrajData *info = &planner_manager_->local_data_;
ros::Time time_now = ros::Time::now();
double t_cur = (time_now - info->start_time_).toSec();
t_cur = min(info->duration_, t_cur);
// 当前位置从轨迹中读取,可改为从odom中读取
Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur);
/* && (end_pt_ - pos).norm() < 0.5 */
if ((target_type_ == TARGET_TYPE::PRESET_TARGET) &&
(wp_id_ < waypoint_num_ - 1) &&
(end_pt_ - pos).norm() < no_replan_thresh_)
{
wp_id_++;
planNextWaypoint(wps_[wp_id_]);
}
else if ((local_target_pt_ - end_pt_).norm() < 1e-3) // end_pt_是目标点，
{
if (t_cur > info->duration_ - 1e-2)
{
have_target_ = false;
have_trigger_ = false;
if (target_type_ == TARGET_TYPE::PRESET_TARGET)
{
wp_id_ = 0;
planNextWaypoint(wps_[wp_id_]);
}
changeFSMExecState(WAIT_TARGET, "FSM");
goto force_return;
// return;
}
else if ((end_pt_ - pos).norm() > no_replan_thresh_ && t_cur > replan_thresh_)
{
changeFSMExecState(REPLAN_TRAJ, "FSM");
}
}
else if (t_cur > replan_thresh_)
{
changeFSMExecState(REPLAN_TRAJ, "FSM");
}
break;
}
case EMERGENCY_STOP:
{
if (flag_escape_emergency_) // Avoiding repeated calls
{
callEmergencyStop(odom_pos_);
}
else
{
if (enable_fail_safe_ && odom_vel_.norm() < 0.1)
changeFSMExecState(GEN_NEW_TRAJ, "FSM");
}
flag_escape_emergency_ = false;
break;
}
}
data_disp_.header.stamp = ros::Time::now();
data_disp_pub_.publish(data_disp_);
force_return:;
exec_timer_.start();
}
bool EGOReplanFSM::planFromGlobalTraj(const int trial_times /*=1*/) //zx-todo
{
start_pt_ = odom_pos_;
start_vel_ = odom_vel_;
start_acc_.setZero();
bool flag_random_poly_init;
// 如果切换了exec状态,timesOfConsecutiveStateCalls().first=1
if (timesOfConsecutiveStateCalls().first == 1)
flag_random_poly_init = false;
else
flag_random_poly_init = true;
for (int i = 0; i < trial_times; i++)
{
if (callReboundReplan(true, flag_random_poly_init))
{
return true;
}
}
return false;
}
bool EGOReplanFSM::planFromCurrentTraj(const int trial_times /*=1*/)
{
LocalTrajData *info = &planner_manager_->local_data_;
ros::Time time_now = ros::Time::now();
double t_cur = (time_now - info->start_time_).toSec();
//cout << "info->velocity_traj_=" << info->velocity_traj_.get_control_points() << endl;
// 将start_pt改为当前位置?
start_pt_ = info->position_traj_.evaluateDeBoorT(t_cur);
start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_cur);
start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_cur);
bool success = callReboundReplan(false, false);
if (!success)
{
success = callReboundReplan(true, false);
//changeFSMExecState(EXEC_TRAJ, "FSM");
if (!success)
{
for (int i = 0; i < trial_times; i++)
{
success = callReboundReplan(true, true);
if (success)
break;
}
if (!success)
{
return false;
}
}
}
return true;
}
void EGOReplanFSM::checkCollisionCallback(const ros::TimerEvent &e)
{
LocalTrajData *info = &planner_manager_->local_data_;
auto map = planner_manager_->grid_map_;
if (exec_state_ == WAIT_TARGET || info->start_time_.toSec() < 1e-5)
return;
/* ---------- check lost of depth ---------- */
if(map->getOdomDepthTimeout())
{
ROS_ERROR("Depth Lost! EMERGENCY_STOP");
enable_fail_safe_ = false;
changeFSMExecState(EMERGENCY_STOP, "SAFETY");
}
else
{
if(enable_fail_safe_ == false)
{
ROS_INFO("Depth Get! GEN_NEW_TRAJ");
enable_fail_safe_ = true;
}
}
/* ---------- check trajectory ---------- */
constexpr double time_step = 0.01;
double t_cur = (ros::Time::now() - info->start_time_).toSec();
Eigen::Vector3d p_cur = info->position_traj_.evaluateDeBoorT(t_cur);
const double CLEARANCE = 1.0 * planner_manager_->getSwarmClearance();
double t_cur_global = ros::Time::now().toSec();
double t_2_3 = info->duration_ * 2 / 3;
for (double t = t_cur; t < info->duration_; t += time_step)
{
if (t_cur < t_2_3 && t >= t_2_3) // If t_cur < t_2_3, only the first 2/3 partition of the trajectory is considered valid and will get checked.
break;
bool occ = false;
occ |= map->getInflateOccupancy(info->position_traj_.evaluateDeBoorT(t));
for (size_t id = 0; id < planner_manager_->swarm_trajs_buf_.size(); id++)
{
if ((planner_manager_->swarm_trajs_buf_.at(id).drone_id != (int)id) || (planner_manager_->swarm_trajs_buf_.at(id).drone_id == planner_manager_->pp_.drone_id))
{
continue;
}
double t_X = t_cur_global - planner_manager_->swarm_trajs_buf_.at(id).start_time_.toSec();
Eigen::Vector3d swarm_pridicted = planner_manager_->swarm_trajs_buf_.at(id).position_traj_.evaluateDeBoorT(t_X);
double dist = (p_cur - swarm_pridicted).norm();
if (dist < CLEARANCE)
{
occ = true;
break;
}
}
if (occ)
{
if (planFromCurrentTraj()) // Make a chance
{
changeFSMExecState(EXEC_TRAJ, "SAFETY");
publishSwarmTrajs(false);
return;
}
else
{
if (t - t_cur < emergency_time_) // 0.8s of emergency time
{
ROS_WARN("Suddenly discovered obstacles. emergency stop! time=%f", t - t_cur);
changeFSMExecState(EMERGENCY_STOP, "SAFETY");
}
else
{
//ROS_WARN("current traj in collision, replan.");
changeFSMExecState(REPLAN_TRAJ, "SAFETY");
}
return;
}
break;
}
}
}
bool EGOReplanFSM::callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj)
{
// 根据planning_horizen_来确定局部目标点 local_target_pt_,local_target_vel_
getLocalTarget();
//
bool plan_and_refine_success =
planner_manager_->reboundReplan(start_pt_, start_vel_, start_acc_, local_target_pt_, local_target_vel_, (have_new_target_ || flag_use_poly_init), flag_randomPolyTraj);
have_new_target_ = false;
// comment
// cout << "refine_success=" << plan_and_refine_success << endl;
if (plan_and_refine_success)
{
auto info = &planner_manager_->local_data_;
traj_utils::Bspline bspline;
bspline.order = 3;
bspline.start_time = info->start_time_;
bspline.traj_id = info->traj_id_;
Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
bspline.pos_pts.reserve(pos_pts.cols());
for (int i = 0; i < pos_pts.cols(); ++i)
{
geometry_msgs::Point pt;
pt.x = pos_pts(0, i);
pt.y = pos_pts(1, i);
pt.z = pos_pts(2, i);
bspline.pos_pts.push_back(pt);
}
Eigen::VectorXd knots = info->position_traj_.getKnot();
// cout << knots.transpose() << endl;
bspline.knots.reserve(knots.rows());
for (int i = 0; i < knots.rows(); ++i)
{
bspline.knots.push_back(knots(i));
}
/* 1. publish traj to traj_server */
bspline_pub_.publish(bspline);
/* 2. publish traj to the next drone of swarm */
/* 3. publish traj for visualization */
// 发布优化后的轨迹 "/optimal_list" , 颜色和scale已经内置
visualization_->displayOptimalList(info->position_traj_.get_control_points(), 0);
}
return plan_and_refine_success;
}
void EGOReplanFSM::publishSwarmTrajs(bool startup_pub)
{
auto info = &planner_manager_->local_data_;
traj_utils::Bspline bspline;
bspline.order = 3;
bspline.start_time = info->start_time_;
bspline.drone_id = planner_manager_->pp_.drone_id;
bspline.traj_id = info->traj_id_;
Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
bspline.pos_pts.reserve(pos_pts.cols());
for (int i = 0; i < pos_pts.cols(); ++i)
{
geometry_msgs::Point pt;
pt.x = pos_pts(0, i);
pt.y = pos_pts(1, i);
pt.z = pos_pts(2, i);
bspline.pos_pts.push_back(pt);
}
Eigen::VectorXd knots = info->position_traj_.getKnot();
// cout << knots.transpose() << endl;
bspline.knots.reserve(knots.rows());
for (int i = 0; i < knots.rows(); ++i)
{
bspline.knots.push_back(knots(i));
}
if (startup_pub)
{
multi_bspline_msgs_buf_.drone_id_from = planner_manager_->pp_.drone_id; // zx-todo
if ((int)multi_bspline_msgs_buf_.traj.size() == planner_manager_->pp_.drone_id + 1)
{
multi_bspline_msgs_buf_.traj.back() = bspline;
}
else if ((int)multi_bspline_msgs_buf_.traj.size() == planner_manager_->pp_.drone_id)
{
multi_bspline_msgs_buf_.traj.push_back(bspline);
}
else
{
ROS_ERROR("Wrong traj nums and drone_id pair!!! traj.size()=%d, drone_id=%d", (int)multi_bspline_msgs_buf_.traj.size(), planner_manager_->pp_.drone_id);
// return plan_and_refine_success;
}
swarm_trajs_pub_.publish(multi_bspline_msgs_buf_);
}
broadcast_bspline_pub_.publish(bspline);
}
bool EGOReplanFSM::callEmergencyStop(Eigen::Vector3d stop_pos)
{
planner_manager_->EmergencyStop(stop_pos);
auto info = &planner_manager_->local_data_;
/* publish traj */
traj_utils::Bspline bspline;
bspline.order = 3;
bspline.start_time = info->start_time_;
bspline.traj_id = info->traj_id_;
Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
bspline.pos_pts.reserve(pos_pts.cols());
for (int i = 0; i < pos_pts.cols(); ++i)
{
geometry_msgs::Point pt;
pt.x = pos_pts(0, i);
pt.y = pos_pts(1, i);
pt.z = pos_pts(2, i);
bspline.pos_pts.push_back(pt);
}
Eigen::VectorXd knots = info->position_traj_.getKnot();
bspline.knots.reserve(knots.rows());
for (int i = 0; i < knots.rows(); ++i)
{
bspline.knots.push_back(knots(i));
}
bspline_pub_.publish(bspline);
return true;
}
void EGOReplanFSM::getLocalTarget()
{
double t;
double t_step = planning_horizen_ / 20 / planner_manager_->pp_.max_vel_;
double dist_min = 9999, dist_min_t = 0.0;
for (t = planner_manager_->global_data_.last_progress_time_; t < planner_manager_->global_data_.global_duration_; t += t_step)
{
Eigen::Vector3d pos_t = planner_manager_->global_data_.getPosition(t);
double dist = (pos_t - start_pt_).norm();
if (t < planner_manager_->global_data_.last_progress_time_ + 1e-5 && dist > planning_horizen_)
{
// Important conor case!
for (; t < planner_manager_->global_data_.global_duration_; t += t_step)
{
Eigen::Vector3d pos_t_temp = planner_manager_->global_data_.getPosition(t);
double dist_temp = (pos_t_temp - start_pt_).norm();
if (dist_temp < planning_horizen_)
{
pos_t = pos_t_temp;
dist = (pos_t - start_pt_).norm();
cout << "Escape conor case \"getLocalTarget\"" << endl;
break;
}
}
}
if (dist < dist_min)
{
dist_min = dist;
dist_min_t = t;
}
if (dist >= planning_horizen_)
{
local_target_pt_ = pos_t;
planner_manager_->global_data_.last_progress_time_ = dist_min_t;
break;
}
}
if (t > planner_manager_->global_data_.global_duration_) // Last global point
{
local_target_pt_ = end_pt_;
planner_manager_->global_data_.last_progress_time_ = planner_manager_->global_data_.global_duration_;
}
if ((end_pt_ - local_target_pt_).norm() < (planner_manager_->pp_.max_vel_ * planner_manager_->pp_.max_vel_) / (2 * planner_manager_->pp_.max_acc_))
{
local_target_vel_ = Eigen::Vector3d::Zero();
}
else
{
local_target_vel_ = planner_manager_->global_data_.getVelocity(t);
}
}
} // namespace ego_planner

600
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/src/planner_manager.cpp
Unescape View File

@ -1,600 +0,0 @@
// #include <fstream>
#include <plan_manage/planner_manager.h>
#include <thread>
#include "visualization_msgs/Marker.h" // zx-todo
namespace ego_planner
{
// SECTION interfaces for setup and query
EGOPlannerManager::EGOPlannerManager() {}
EGOPlannerManager::~EGOPlannerManager() {}
void EGOPlannerManager::initPlanModules(ros::NodeHandle &nh, PlanningVisualization::Ptr vis)
{
/* read algorithm parameters */
// 最大速度
nh.param("manager/max_vel", pp_.max_vel_, -1.0);
// 最大加速度
nh.param("manager/max_acc", pp_.max_acc_, -1.0);
// 最大加加速度
nh.param("manager/max_jerk", pp_.max_jerk_, -1.0);
// 未知，限制参数
nh.param("manager/feasibility_tolerance", pp_.feasibility_tolerance_, 0.0);
// 控制点距离
nh.param("manager/control_points_distance", pp_.ctrl_pt_dist, -1.0);
// 规划范围
nh.param("manager/planning_horizon", pp_.planning_horizen_, 5.0);
// ？
nh.param("manager/use_distinctive_trajs", pp_.use_distinctive_trajs, false);
// 无人机ID
nh.param("manager/drone_id", pp_.drone_id, 0);
local_data_.traj_id_ = 0;
// 地图类
grid_map_.reset(new GridMap);
grid_map_->initMap(nh);
// 默认没有使用
// obj_predictor_.reset(new fast_planner::ObjPredictor(nh));
// obj_predictor_->init();
// obj_pub_ = nh.advertise<visualization_msgs::Marker>("/dynamic/obj_prdi", 10); // zx-todo
// bspline优化器
bspline_optimizer_.reset(new BsplineOptimizer);
bspline_optimizer_->setParam(nh);
bspline_optimizer_->setEnvironment(grid_map_, obj_predictor_);
bspline_optimizer_->a_star_.reset(new AStar);
bspline_optimizer_->a_star_->initGridMap(grid_map_, Eigen::Vector3i(100, 100, 100));
visualization_ = vis;
}
// !SECTION
// SECTION rebond replanning
bool EGOPlannerManager::reboundReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,
Eigen::Vector3d start_acc, Eigen::Vector3d local_target_pt,
Eigen::Vector3d local_target_vel, bool flag_polyInit, bool flag_randomPolyTraj)
{
static int count = 0;
// comment
// printf("\033[47;30m\n[drone no.%d , replan %d times]====================================\033[0m\n", pp_.drone_id, count++);
// cout.precision(3);
// cout << "start: " << start_pt.transpose() << ", " << start_vel.transpose() << "\ngoal:" << local_target_pt.transpose() << ", " << local_target_vel.transpose()
// << endl;
if ((start_pt - local_target_pt).norm() < 0.2)
{
cout << "The start point is too Close to goal" << endl;
continous_failures_count_++;
return false;
}
bspline_optimizer_->setLocalTargetPt(local_target_pt);
ros::Time t_start = ros::Time::now();
ros::Duration t_init, t_opt, t_refine;
/*** STEP 1: INIT ***/
double ts = (start_pt - local_target_pt).norm() > 0.1 ? pp_.ctrl_pt_dist / pp_.max_vel_ * 1.5 : pp_.ctrl_pt_dist / pp_.max_vel_ * 5; // pp_.ctrl_pt_dist / pp_.max_vel_ is too tense, and will surely exceed the acc/vel limits
vector<Eigen::Vector3d> point_set, start_end_derivatives;
static bool flag_first_call = true, flag_force_polynomial = false;
bool flag_regenerate = false;
do
{
point_set.clear();
start_end_derivatives.clear();
flag_regenerate = false;
if (flag_first_call || flag_polyInit || flag_force_polynomial /*|| ( start_pt - local_target_pt ).norm() < 1.0*/) // Initial path generated from a min-snap traj by order.
{
flag_first_call = false;
flag_force_polynomial = false;
PolynomialTraj gl_traj;
double dist = (start_pt - local_target_pt).norm();
double time = pow(pp_.max_vel_, 2) / pp_.max_acc_ > dist ? sqrt(dist / pp_.max_acc_) : (dist - pow(pp_.max_vel_, 2) / pp_.max_acc_) / pp_.max_vel_ + 2 * pp_.max_vel_ / pp_.max_acc_;
if (!flag_randomPolyTraj)
{
gl_traj = PolynomialTraj::one_segment_traj_gen(start_pt, start_vel, start_acc, local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), time);
}
else
{
Eigen::Vector3d horizen_dir = ((start_pt - local_target_pt).cross(Eigen::Vector3d(0, 0, 1))).normalized();
Eigen::Vector3d vertical_dir = ((start_pt - local_target_pt).cross(horizen_dir)).normalized();
Eigen::Vector3d random_inserted_pt = (start_pt + local_target_pt) / 2 +
(((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * horizen_dir * 0.8 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989) +
(((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * vertical_dir * 0.4 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989);
Eigen::MatrixXd pos(3, 3);
pos.col(0) = start_pt;
pos.col(1) = random_inserted_pt;
pos.col(2) = local_target_pt;
Eigen::VectorXd t(2);
t(0) = t(1) = time / 2;
gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, local_target_vel, start_acc, Eigen::Vector3d::Zero(), t);
}
double t;
bool flag_too_far;
ts *= 1.5; // ts will be divided by 1.5 in the next
do
{
ts /= 1.5;
point_set.clear();
flag_too_far = false;
Eigen::Vector3d last_pt = gl_traj.evaluate(0);
for (t = 0; t < time; t += ts)
{
Eigen::Vector3d pt = gl_traj.evaluate(t);
if ((last_pt - pt).norm() > pp_.ctrl_pt_dist * 1.5)
{
flag_too_far = true;
break;
}
last_pt = pt;
point_set.push_back(pt);
}
} while (flag_too_far || point_set.size() < 7); // To make sure the initial path has enough points.
t -= ts;
start_end_derivatives.push_back(gl_traj.evaluateVel(0));
start_end_derivatives.push_back(local_target_vel);
start_end_derivatives.push_back(gl_traj.evaluateAcc(0));
start_end_derivatives.push_back(gl_traj.evaluateAcc(t));
}
else // Initial path generated from previous trajectory.
{
double t;
double t_cur = (ros::Time::now() - local_data_.start_time_).toSec();
vector<double> pseudo_arc_length;
vector<Eigen::Vector3d> segment_point;
pseudo_arc_length.push_back(0.0);
for (t = t_cur; t < local_data_.duration_ + 1e-3; t += ts)
{
segment_point.push_back(local_data_.position_traj_.evaluateDeBoorT(t));
if (t > t_cur)
{
pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back());
}
}
t -= ts;
double poly_time = (local_data_.position_traj_.evaluateDeBoorT(t) - local_target_pt).norm() / pp_.max_vel_ * 2;
if (poly_time > ts)
{
PolynomialTraj gl_traj = PolynomialTraj::one_segment_traj_gen(local_data_.position_traj_.evaluateDeBoorT(t),
local_data_.velocity_traj_.evaluateDeBoorT(t),
local_data_.acceleration_traj_.evaluateDeBoorT(t),
local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), poly_time);
for (t = ts; t < poly_time; t += ts)
{
if (!pseudo_arc_length.empty())
{
segment_point.push_back(gl_traj.evaluate(t));
pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back());
}
else
{
ROS_ERROR("pseudo_arc_length is empty, return!");
continous_failures_count_++;
return false;
}
}
}
double sample_length = 0;
double cps_dist = pp_.ctrl_pt_dist * 1.5; // cps_dist will be divided by 1.5 in the next
size_t id = 0;
do
{
cps_dist /= 1.5;
point_set.clear();
sample_length = 0;
id = 0;
while ((id <= pseudo_arc_length.size() - 2) && sample_length <= pseudo_arc_length.back())
{
if (sample_length >= pseudo_arc_length[id] && sample_length < pseudo_arc_length[id + 1])
{
point_set.push_back((sample_length - pseudo_arc_length[id]) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id + 1] +
(pseudo_arc_length[id + 1] - sample_length) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id]);
sample_length += cps_dist;
}
else
id++;
}
point_set.push_back(local_target_pt);
} while (point_set.size() < 7); // If the start point is very close to end point, this will help
start_end_derivatives.push_back(local_data_.velocity_traj_.evaluateDeBoorT(t_cur));
start_end_derivatives.push_back(local_target_vel);
start_end_derivatives.push_back(local_data_.acceleration_traj_.evaluateDeBoorT(t_cur));
start_end_derivatives.push_back(Eigen::Vector3d::Zero());
if (point_set.size() > pp_.planning_horizen_ / pp_.ctrl_pt_dist * 3) // The initial path is unnormally too long!
{
flag_force_polynomial = true;
flag_regenerate = true;
}
}
} while (flag_regenerate);
Eigen::MatrixXd ctrl_pts, ctrl_pts_temp;
UniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts);
vector<std::pair<int, int>> segments;
segments = bspline_optimizer_->initControlPoints(ctrl_pts, true);
t_init = ros::Time::now() - t_start;
t_start = ros::Time::now();
/*** STEP 2: OPTIMIZE ***/
bool flag_step_1_success = false;
vector<vector<Eigen::Vector3d>> vis_trajs;
if (pp_.use_distinctive_trajs)
{
// cout << "enter" << endl;
std::vector<ControlPoints> trajs = bspline_optimizer_->distinctiveTrajs(segments);
// comment
// cout << "\033[1;33m"<< "multi-trajs=" << trajs.size() << "\033[1;0m" << endl;
double final_cost, min_cost = 999999.0;
for (int i = trajs.size() - 1; i >= 0; i--)
{
if (bspline_optimizer_->BsplineOptimizeTrajRebound(ctrl_pts_temp, final_cost, trajs[i], ts))
{
//comment
// cout << "traj " << trajs.size() - i << " success." << endl;
flag_step_1_success = true;
if (final_cost < min_cost)
{
min_cost = final_cost;
ctrl_pts = ctrl_pts_temp;
}
// visualization
point_set.clear();
for (int j = 0; j < ctrl_pts_temp.cols(); j++)
{
point_set.push_back(ctrl_pts_temp.col(j));
}
vis_trajs.push_back(point_set);
}
else
{
cout << "traj " << trajs.size() - i << " failed." << endl;
}
}
t_opt = ros::Time::now() - t_start;
// 显示 "/init_list"
visualization_->displayMultiInitPathList(vis_trajs, 0.2); // This visuallization will take up several milliseconds.
}
else
{
flag_step_1_success = bspline_optimizer_->BsplineOptimizeTrajRebound(ctrl_pts, ts);
t_opt = ros::Time::now() - t_start;
//static int vis_id = 0;
visualization_->displayInitPathList(point_set, 0.2, 0);
}
// comment
// cout << "/uav"<< pp_.drone_id<< " plan_success=" << flag_step_1_success << endl;
if (!flag_step_1_success)
{
visualization_->displayOptimalList(ctrl_pts, 0);
continous_failures_count_++;
return false;
}
t_start = ros::Time::now();
UniformBspline pos = UniformBspline(ctrl_pts, 3, ts);
pos.setPhysicalLimits(pp_.max_vel_, pp_.max_acc_, pp_.feasibility_tolerance_);
/*** STEP 3: REFINE(RE-ALLOCATE TIME) IF NECESSARY ***/
// Note: Only adjust time in single drone mode. But we still allow drone_0 to adjust its time profile.
// ego默认从0开始，我们默认从1开始，因此这里<=1
if (pp_.drone_id <= 1)
{
double ratio;
bool flag_step_2_success = true;
if (!pos.checkFeasibility(ratio, false))
{
cout << "Need to reallocate time." << endl;
Eigen::MatrixXd optimal_control_points;
flag_step_2_success = refineTrajAlgo(pos, start_end_derivatives, ratio, ts, optimal_control_points);
if (flag_step_2_success)
pos = UniformBspline(optimal_control_points, 3, ts);
}
if (!flag_step_2_success)
{
printf("\033[34mThis refined trajectory hits obstacles. It doesn't matter if appeares occasionally. But if continously appearing, Increase parameter \"lambda_fitness\".\n\033[0m");
continous_failures_count_++;
return false;
}
}
else
{
static bool print_once = true;
if (print_once)
{
print_once = false;
ROS_ERROR("IN SWARM MODE, REFINE DISABLED!");
}
}
t_refine = ros::Time::now() - t_start;
// save planned results
updateTrajInfo(pos, ros::Time::now());
static double sum_time = 0;
static int count_success = 0;
sum_time += (t_init + t_opt + t_refine).toSec();
count_success++;
// comment
// cout << "total time:\033[42m" << (t_init + t_opt + t_refine).toSec() << "\033[0m,optimize:" << (t_init + t_opt).toSec() << ",refine:" << t_refine.toSec() << ",avg_time=" << sum_time / count_success << endl;
// success. YoY
continous_failures_count_ = 0;
return true;
}
bool EGOPlannerManager::EmergencyStop(Eigen::Vector3d stop_pos)
{
Eigen::MatrixXd control_points(3, 6);
for (int i = 0; i < 6; i++)
{
control_points.col(i) = stop_pos;
}
updateTrajInfo(UniformBspline(control_points, 3, 1.0), ros::Time::now());
return true;
}
bool EGOPlannerManager::checkCollision(int drone_id)
{
if (local_data_.start_time_.toSec() < 1e9) // It means my first planning has not started
return false;
double my_traj_start_time = local_data_.start_time_.toSec();
double other_traj_start_time = swarm_trajs_buf_[drone_id].start_time_.toSec();
double t_start = max(my_traj_start_time, other_traj_start_time);
double t_end = min(my_traj_start_time + local_data_.duration_ * 2 / 3, other_traj_start_time + swarm_trajs_buf_[drone_id].duration_);
for (double t = t_start; t < t_end; t += 0.03)
{
if ((local_data_.position_traj_.evaluateDeBoorT(t - my_traj_start_time) - swarm_trajs_buf_[drone_id].position_traj_.evaluateDeBoorT(t - other_traj_start_time)).norm() < bspline_optimizer_->getSwarmClearance())
{
return true;
}
}
return false;
}
bool EGOPlannerManager::planGlobalTrajWaypoints(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
const std::vector<Eigen::Vector3d> &waypoints, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc)
{
// generate global reference trajectory
vector<Eigen::Vector3d> points;
points.push_back(start_pos);
for (size_t wp_i = 0; wp_i < waypoints.size(); wp_i++)
{
points.push_back(waypoints[wp_i]);
}
double total_len = 0;
total_len += (start_pos - waypoints[0]).norm();
for (size_t i = 0; i < waypoints.size() - 1; i++)
{
total_len += (waypoints[i + 1] - waypoints[i]).norm();
}
// insert intermediate points if too far
vector<Eigen::Vector3d> inter_points;
double dist_thresh = max(total_len / 8, 4.0);
for (size_t i = 0; i < points.size() - 1; ++i)
{
inter_points.push_back(points.at(i));
double dist = (points.at(i + 1) - points.at(i)).norm();
if (dist > dist_thresh)
{
int id_num = floor(dist / dist_thresh) + 1;
for (int j = 1; j < id_num; ++j)
{
Eigen::Vector3d inter_pt =
points.at(i) * (1.0 - double(j) / id_num) + points.at(i + 1) * double(j) / id_num;
inter_points.push_back(inter_pt);
}
}
}
inter_points.push_back(points.back());
// for ( int i=0; i<inter_points.size(); i++ )
// {
// cout << inter_points[i].transpose() << endl;
// }
// write position matrix
int pt_num = inter_points.size();
Eigen::MatrixXd pos(3, pt_num);
for (int i = 0; i < pt_num; ++i)
pos.col(i) = inter_points[i];
Eigen::Vector3d zero(0, 0, 0);
Eigen::VectorXd time(pt_num - 1);
for (int i = 0; i < pt_num - 1; ++i)
{
time(i) = (pos.col(i + 1) - pos.col(i)).norm() / (pp_.max_vel_);
}
time(0) *= 2.0;
time(time.rows() - 1) *= 2.0;
PolynomialTraj gl_traj;
if (pos.cols() >= 3)
gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, end_vel, start_acc, end_acc, time);
else if (pos.cols() == 2)
gl_traj = PolynomialTraj::one_segment_traj_gen(start_pos, start_vel, start_acc, pos.col(1), end_vel, end_acc, time(0));
else
return false;
auto time_now = ros::Time::now();
global_data_.setGlobalTraj(gl_traj, time_now);
return true;
}
bool EGOPlannerManager::planGlobalTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
const Eigen::Vector3d &end_pos, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc)
{
// generate global reference trajectory,生成全局参考轨迹
vector<Eigen::Vector3d> points;
points.push_back(start_pos);
points.push_back(end_pos);
// insert intermediate points if too far
vector<Eigen::Vector3d> inter_points;
const double dist_thresh = 4.0;
// points.size() = 2, 这个for循环只执行一次?
for (size_t i = 0; i < points.size() - 1; ++i)
{
// push_back初始点
inter_points.push_back(points.at(i));
// 目标点与起始点的距离
double dist = (points.at(i + 1) - points.at(i)).norm();
// 距离大于4米
if (dist > dist_thresh)
{
// 以4米为一个单位计算增加的点数
int id_num = floor(dist / dist_thresh) + 1;
for (int j = 1; j < id_num; ++j)
{
Eigen::Vector3d inter_pt =
points.at(i) * (1.0 - double(j) / id_num) + points.at(i + 1) * double(j) / id_num;
inter_points.push_back(inter_pt);
}
}
}
// push_back目标点
inter_points.push_back(points.back());
// write position matrix
int pt_num = inter_points.size();
// 航点矩阵
Eigen::MatrixXd pos(3, pt_num);
// 赋值
for (int i = 0; i < pt_num; ++i)
pos.col(i) = inter_points[i];
Eigen::Vector3d zero(0, 0, 0);
Eigen::VectorXd time(pt_num - 1);
for (int i = 0; i < pt_num - 1; ++i)
{
// 预估时间
time(i) = (pos.col(i + 1) - pos.col(i)).norm() / (pp_.max_vel_);
}
// 第一段时间扩大2倍
time(0) *= 2.0;
// 最后一段时间扩大2倍
time(time.rows() - 1) *= 2.0;
PolynomialTraj gl_traj;
// 如果仅有一段,或有多段
if (pos.cols() >= 3)
gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, end_vel, start_acc, end_acc, time);
else if (pos.cols() == 2)
gl_traj = PolynomialTraj::one_segment_traj_gen(start_pos, start_vel, start_acc, end_pos, end_vel, end_acc, time(0));
else
return false;
auto time_now = ros::Time::now();
// 存入global_data_,等待下一步处理
global_data_.setGlobalTraj(gl_traj, time_now);
return true;
}
bool EGOPlannerManager::refineTrajAlgo(UniformBspline &traj, vector<Eigen::Vector3d> &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points)
{
double t_inc;
Eigen::MatrixXd ctrl_pts; // = traj.getControlPoint()
// std::cout << "ratio: " << ratio << std::endl;
reparamBspline(traj, start_end_derivative, ratio, ctrl_pts, ts, t_inc);
traj = UniformBspline(ctrl_pts, 3, ts);
double t_step = traj.getTimeSum() / (ctrl_pts.cols() - 3);
bspline_optimizer_->ref_pts_.clear();
for (double t = 0; t < traj.getTimeSum() + 1e-4; t += t_step)
bspline_optimizer_->ref_pts_.push_back(traj.evaluateDeBoorT(t));
bool success = bspline_optimizer_->BsplineOptimizeTrajRefine(ctrl_pts, ts, optimal_control_points);
return success;
}
void EGOPlannerManager::updateTrajInfo(const UniformBspline &position_traj, const ros::Time time_now)
{
local_data_.start_time_ = time_now;
local_data_.position_traj_ = position_traj;
local_data_.velocity_traj_ = local_data_.position_traj_.getDerivative();
local_data_.acceleration_traj_ = local_data_.velocity_traj_.getDerivative();
local_data_.start_pos_ = local_data_.position_traj_.evaluateDeBoorT(0.0);
local_data_.duration_ = local_data_.position_traj_.getTimeSum();
local_data_.traj_id_ += 1;
}
void EGOPlannerManager::reparamBspline(UniformBspline &bspline, vector<Eigen::Vector3d> &start_end_derivative, double ratio,
Eigen::MatrixXd &ctrl_pts, double &dt, double &time_inc)
{
double time_origin = bspline.getTimeSum();
int seg_num = bspline.getControlPoint().cols() - 3;
// double length = bspline.getLength(0.1);
// int seg_num = ceil(length / pp_.ctrl_pt_dist);
bspline.lengthenTime(ratio);
double duration = bspline.getTimeSum();
dt = duration / double(seg_num);
time_inc = duration - time_origin;
vector<Eigen::Vector3d> point_set;
for (double time = 0.0; time <= duration + 1e-4; time += dt)
{
point_set.push_back(bspline.evaluateDeBoorT(time));
}
UniformBspline::parameterizeToBspline(dt, point_set, start_end_derivative, ctrl_pts);
}
} // namespace ego_planner

264
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/plan_manage/src/traj_server.cpp
Unescape View File

@ -1,264 +0,0 @@
#include "bspline_opt/uniform_bspline.h"
#include "nav_msgs/Odometry.h"
#include "traj_utils/Bspline.h"
#include "quadrotor_msgs/PositionCommand.h"
#include "std_msgs/Empty.h"
#include "visualization_msgs/Marker.h"
#include <ros/ros.h>
ros::Publisher pos_cmd_pub;
quadrotor_msgs::PositionCommand cmd;
double pos_gain[3] = {0, 0, 0};
double vel_gain[3] = {0, 0, 0};
using ego_planner::UniformBspline;
bool receive_traj_ = false;
vector<UniformBspline> traj_;
double traj_duration_;
ros::Time start_time_;
int traj_id_;
// yaw control
double last_yaw_, last_yaw_dot_;
double time_forward_;
void bsplineCallback(traj_utils::BsplineConstPtr msg)
{
// parse pos traj
Eigen::MatrixXd pos_pts(3, msg->pos_pts.size());
Eigen::VectorXd knots(msg->knots.size());
for (size_t i = 0; i < msg->knots.size(); ++i)
{
knots(i) = msg->knots[i];
}
for (size_t i = 0; i < msg->pos_pts.size(); ++i)
{
pos_pts(0, i) = msg->pos_pts[i].x;
pos_pts(1, i) = msg->pos_pts[i].y;
pos_pts(2, i) = msg->pos_pts[i].z;
}
UniformBspline pos_traj(pos_pts, msg->order, 0.1);
pos_traj.setKnot(knots);
// parse yaw traj
// Eigen::MatrixXd yaw_pts(msg->yaw_pts.size(), 1);
// for (int i = 0; i < msg->yaw_pts.size(); ++i) {
// yaw_pts(i, 0) = msg->yaw_pts[i];
// }
//UniformBspline yaw_traj(yaw_pts, msg->order, msg->yaw_dt);
start_time_ = msg->start_time;
traj_id_ = msg->traj_id;
traj_.clear();
traj_.push_back(pos_traj);
traj_.push_back(traj_[0].getDerivative());
traj_.push_back(traj_[1].getDerivative());
traj_duration_ = traj_[0].getTimeSum();
receive_traj_ = true;
}
std::pair<double, double> calculate_yaw(double t_cur, Eigen::Vector3d &pos, ros::Time &time_now, ros::Time &time_last)
{
constexpr double PI = 3.1415926;
constexpr double YAW_DOT_MAX_PER_SEC = PI;
// constexpr double YAW_DOT_DOT_MAX_PER_SEC = PI;
std::pair<double, double> yaw_yawdot(0, 0);
double yaw = 0;
double yawdot = 0;
Eigen::Vector3d dir = t_cur + time_forward_ <= traj_duration_ ? traj_[0].evaluateDeBoorT(t_cur + time_forward_) - pos : traj_[0].evaluateDeBoorT(traj_duration_) - pos;
double yaw_temp = dir.norm() > 0.1 ? atan2(dir(1), dir(0)) : last_yaw_;
double max_yaw_change = YAW_DOT_MAX_PER_SEC * (time_now - time_last).toSec();
if (yaw_temp - last_yaw_ > PI)
{
if (yaw_temp - last_yaw_ - 2 * PI < -max_yaw_change)
{
yaw = last_yaw_ - max_yaw_change;
if (yaw < -PI)
yaw += 2 * PI;
yawdot = -YAW_DOT_MAX_PER_SEC;
}
else
{
yaw = yaw_temp;
if (yaw - last_yaw_ > PI)
yawdot = -YAW_DOT_MAX_PER_SEC;
else
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
}
}
else if (yaw_temp - last_yaw_ < -PI)
{
if (yaw_temp - last_yaw_ + 2 * PI > max_yaw_change)
{
yaw = last_yaw_ + max_yaw_change;
if (yaw > PI)
yaw -= 2 * PI;
yawdot = YAW_DOT_MAX_PER_SEC;
}
else
{
yaw = yaw_temp;
if (yaw - last_yaw_ < -PI)
yawdot = YAW_DOT_MAX_PER_SEC;
else
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
}
}
else
{
if (yaw_temp - last_yaw_ < -max_yaw_change)
{
yaw = last_yaw_ - max_yaw_change;
if (yaw < -PI)
yaw += 2 * PI;
yawdot = -YAW_DOT_MAX_PER_SEC;
}
else if (yaw_temp - last_yaw_ > max_yaw_change)
{
yaw = last_yaw_ + max_yaw_change;
if (yaw > PI)
yaw -= 2 * PI;
yawdot = YAW_DOT_MAX_PER_SEC;
}
else
{
yaw = yaw_temp;
if (yaw - last_yaw_ > PI)
yawdot = -YAW_DOT_MAX_PER_SEC;
else if (yaw - last_yaw_ < -PI)
yawdot = YAW_DOT_MAX_PER_SEC;
else
yawdot = (yaw_temp - last_yaw_) / (time_now - time_last).toSec();
}
}
if (fabs(yaw - last_yaw_) <= max_yaw_change)
yaw = 0.5 * last_yaw_ + 0.5 * yaw; // nieve LPF
yawdot = 0.5 * last_yaw_dot_ + 0.5 * yawdot;
last_yaw_ = yaw;
last_yaw_dot_ = yawdot;
yaw_yawdot.first = yaw;
yaw_yawdot.second = yawdot;
return yaw_yawdot;
}
void cmdCallback(const ros::TimerEvent &e)
{
/* no publishing before receive traj_ */
if (!receive_traj_)
return;
ros::Time time_now = ros::Time::now();
double t_cur = (time_now - start_time_).toSec();
Eigen::Vector3d pos(Eigen::Vector3d::Zero()), vel(Eigen::Vector3d::Zero()), acc(Eigen::Vector3d::Zero()), pos_f;
std::pair<double, double> yaw_yawdot(0, 0);
static ros::Time time_last = ros::Time::now();
if (t_cur < traj_duration_ && t_cur >= 0.0)
{
pos = traj_[0].evaluateDeBoorT(t_cur);
vel = traj_[1].evaluateDeBoorT(t_cur);
acc = traj_[2].evaluateDeBoorT(t_cur);
/*** calculate yaw ***/
yaw_yawdot = calculate_yaw(t_cur, pos, time_now, time_last);
/*** calculate yaw ***/
double tf = min(traj_duration_, t_cur + 2.0);
pos_f = traj_[0].evaluateDeBoorT(tf);
}
else if (t_cur >= traj_duration_)
{
/* hover when finish traj_ */
pos = traj_[0].evaluateDeBoorT(traj_duration_);
vel.setZero();
acc.setZero();
yaw_yawdot.first = last_yaw_;
yaw_yawdot.second = 0;
pos_f = pos;
}
else
{
cout << "[Traj server]: invalid time." << endl;
}
time_last = time_now;
cmd.header.stamp = time_now;
cmd.header.frame_id = "world";
cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY;
cmd.trajectory_id = traj_id_;
cmd.position.x = pos(0);
cmd.position.y = pos(1);
cmd.position.z = pos(2);
cmd.velocity.x = vel(0);
cmd.velocity.y = vel(1);
cmd.velocity.z = vel(2);
cmd.acceleration.x = acc(0);
cmd.acceleration.y = acc(1);
cmd.acceleration.z = acc(2);
cmd.yaw = yaw_yawdot.first;
cmd.yaw_dot = yaw_yawdot.second;
last_yaw_ = cmd.yaw;
pos_cmd_pub.publish(cmd);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "traj_server");
// ros::NodeHandle node;
ros::NodeHandle nh("~");
ros::Subscriber bspline_sub = nh.subscribe("planning/bspline", 10, bsplineCallback);
pos_cmd_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 50);
ros::Timer cmd_timer = nh.createTimer(ros::Duration(0.01), cmdCallback);
/* control parameter */
cmd.kx[0] = pos_gain[0];
cmd.kx[1] = pos_gain[1];
cmd.kx[2] = pos_gain[2];
cmd.kv[0] = vel_gain[0];
cmd.kv[1] = vel_gain[1];
cmd.kv[2] = vel_gain[2];
nh.param("traj_server/time_forward", time_forward_, -1.0);
nh.param("traj_server/last_yaw", last_yaw_, 0.0);
last_yaw_dot_ = 0.0;
ros::Duration(1.0).sleep();
ROS_WARN("[Traj server]: ready.");
ros::spin();
return 0;
}

35
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/rosmsg_tcp_bridge/CMakeLists.txt
Unescape View File

@ -1,35 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(rosmsg_tcp_bridge)
set(CMAKE_BUILD_TYPE "Release")
ADD_COMPILE_OPTIONS(-std=c++11 )
ADD_COMPILE_OPTIONS(-std=c++14 )
set(CMAKE_CXX_FLAGS_RELEASE "-O2 -Wall -g")
find_package(Eigen3 REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
geometry_msgs
traj_utils
)
catkin_package(
CATKIN_DEPENDS traj_utils
)
include_directories(
SYSTEM
${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIR}
)
add_executable(bridge_node
src/bridge_node.cpp
)
target_link_libraries(bridge_node
${catkin_LIBRARIES}
)

18
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/rosmsg_tcp_bridge/launch/bridge.launch
Unescape View File

@ -1,18 +0,0 @@
<launch>
<arg name="drone_id" value="1" />
<!-- <node pkg="rosmsg_tcp_bridge" name="drone_$(arg drone_id)_bridge_node" type="bridge_node" output="screen" launch-prefix="valgrind" > -->
<node pkg="rosmsg_tcp_bridge" name="drone_$(arg drone_id)_bridge_node" type="bridge_node" output="screen" >
<remap from="position_cmd" to="drone_$(arg drone_id)_planning/pos_cmd"/>
<remap from="planning/bspline" to="drone_$(arg drone_id)_planning/bspline"/>
<remap from="~my_odom" to="/vins_estimator/imu_propagate"/>
<param name="next_drone_ip" value="127.0.0.1" type="string"/>
<param name="broadcast_ip" value="127.0.0.255" type="string"/>
<param name="drone_id" value="$(arg drone_id)"/>
<param name="odom_max_freq" value="70"/>
</node>
</launch>

67
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/rosmsg_tcp_bridge/package.xml
Unescape View File

@ -1,67 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>rosmsg_tcp_bridge</name>
<version>0.0.0</version>
<description>The rosmsg_tcp_bridge package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="iszhouxin@zju.edu.cn">iszhouxin</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ego_planner</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>traj_utils</build_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>traj_utils</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

828
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/rosmsg_tcp_bridge/src/bridge_node.cpp
Unescape View File

@ -1,828 +0,0 @@
#include <ros/ros.h>
#include <boost/thread.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <traj_utils/MultiBsplines.h>
#include <traj_utils/Bspline.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Empty.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <netinet/in.h>
#define PORT 8080
#define UDP_PORT 8081
#define BUF_LEN 1048576 // 1MB
#define BUF_LEN_SHORT 1024 // 1KB
using namespace std;
int send_sock_, server_fd_, recv_sock_, udp_server_fd_, udp_send_fd_;
ros::Subscriber swarm_trajs_sub_, other_odoms_sub_, emergency_stop_sub_, one_traj_sub_;
ros::Publisher swarm_trajs_pub_, other_odoms_pub_, emergency_stop_pub_, one_traj_pub_;
string tcp_ip_, udp_ip_;
int drone_id_;
double odom_broadcast_freq_;
char send_buf_[BUF_LEN], recv_buf_[BUF_LEN], udp_recv_buf_[BUF_LEN], udp_send_buf_[BUF_LEN];
struct sockaddr_in addr_udp_send_;
traj_utils::MultiBsplinesPtr bsplines_msg_;
nav_msgs::OdometryPtr odom_msg_;
std_msgs::EmptyPtr stop_msg_;
traj_utils::BsplinePtr bspline_msg_;
enum MESSAGE_TYPE
{
ODOM = 888,
MULTI_TRAJ,
ONE_TRAJ,
STOP
} massage_type_;
int connect_to_next_drone(const char *ip, const int port)
{
/* Connect */
int sock = 0;
struct sockaddr_in serv_addr;
if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0)
{
printf("\n Socket creation error \n");
return -1;
}
serv_addr.sin_family = AF_INET;
serv_addr.sin_port = htons(port);
// Convert IPv4 and IPv6 addresses from text to binary form
if (inet_pton(AF_INET, ip, &serv_addr.sin_addr) <= 0)
{
printf("\nInvalid address/ Address not supported \n");
return -1;
}
if (connect(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
{
ROS_WARN("Tcp connection to drone_%d Failed", drone_id_+1);
return -1;
}
char str[INET_ADDRSTRLEN];
ROS_INFO("Connect to %s success!", inet_ntop(AF_INET, &serv_addr.sin_addr, str, sizeof(str)));
return sock;
}
int init_broadcast(const char *ip, const int port)
{
int fd;
if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) <= 0)
{
ROS_ERROR("[bridge_node]Socket sender creation error!");
exit(EXIT_FAILURE);
}
int so_broadcast = 1;
if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &so_broadcast, sizeof(so_broadcast)) < 0)
{
cout << "Error in setting Broadcast option";
exit(EXIT_FAILURE);
}
addr_udp_send_.sin_family = AF_INET;
addr_udp_send_.sin_port = htons(port);
if (inet_pton(AF_INET, ip, &addr_udp_send_.sin_addr) <= 0)
{
printf("\nInvalid address/ Address not supported \n");
return -1;
}
return fd;
}
int wait_connection_from_previous_drone(const int port, int &server_fd, int &new_socket)
{
struct sockaddr_in address;
int opt = 1;
int addrlen = sizeof(address);
// Creating socket file descriptor
if ((server_fd = socket(AF_INET, SOCK_STREAM, 0)) == 0)
{
perror("socket failed");
exit(EXIT_FAILURE);
}
// Forcefully attaching socket to the port
if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT,
&opt, sizeof(opt)))
{
perror("setsockopt");
exit(EXIT_FAILURE);
}
address.sin_family = AF_INET;
address.sin_addr.s_addr = INADDR_ANY;
address.sin_port = htons(PORT);
// Forcefully attaching socket to the port
if (bind(server_fd, (struct sockaddr *)&address,
sizeof(address)) < 0)
{
perror("bind failed");
exit(EXIT_FAILURE);
}
if (listen(server_fd, 3) < 0)
{
perror("listen");
exit(EXIT_FAILURE);
}
if ((new_socket = accept(server_fd, (struct sockaddr *)&address,
(socklen_t *)&addrlen)) < 0)
{
perror("accept");
exit(EXIT_FAILURE);
}
char str[INET_ADDRSTRLEN];
ROS_INFO( "Receive tcp connection from %s", inet_ntop(AF_INET, &address.sin_addr, str, sizeof(str)) );
return new_socket;
}
int udp_bind_to_port(const int port, int &server_fd)
{
struct sockaddr_in address;
int opt = 1;
// Creating socket file descriptor
if ((server_fd = socket(AF_INET, SOCK_DGRAM, 0)) == 0)
{
perror("socket failed");
exit(EXIT_FAILURE);
}
// Forcefully attaching socket to the port
if (setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR | SO_REUSEPORT,
&opt, sizeof(opt)))
{
perror("setsockopt");
exit(EXIT_FAILURE);
}
address.sin_family = AF_INET;
address.sin_addr.s_addr = INADDR_ANY;
address.sin_port = htons(port);
// Forcefully attaching socket to the port
if (bind(server_fd, (struct sockaddr *)&address,
sizeof(address)) < 0)
{
perror("bind failed");
exit(EXIT_FAILURE);
}
return server_fd;
}
int serializeMultiBsplines(const traj_utils::MultiBsplinesPtr &msg)
{
char *ptr = send_buf_;
unsigned long total_len = 0;
total_len += sizeof(MESSAGE_TYPE) + sizeof(int32_t) + sizeof(size_t);
for (size_t i = 0; i < msg->traj.size(); i++)
{
total_len += sizeof(int32_t) + sizeof(int32_t) + sizeof(double) + sizeof(int64_t) + sizeof(double);
total_len += sizeof(size_t) + msg->traj[i].knots.size() * sizeof(double);
total_len += sizeof(size_t) + (3 * msg->traj[i].pos_pts.size()) * sizeof(double);
total_len += sizeof(size_t) + msg->traj[i].yaw_pts.size() * sizeof(double);
}
if (total_len + 1 > BUF_LEN)
{
ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN");
return -1;
}
*((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::MULTI_TRAJ;
ptr += sizeof(MESSAGE_TYPE);
*((int32_t *)ptr) = msg->drone_id_from;
ptr += sizeof(int32_t);
if (ptr - send_buf_ > BUF_LEN)
{
}
*((size_t *)ptr) = msg->traj.size();
ptr += sizeof(size_t);
for (size_t i = 0; i < msg->traj.size(); i++)
{
*((int32_t *)ptr) = msg->traj[i].drone_id;
ptr += sizeof(int32_t);
*((int32_t *)ptr) = msg->traj[i].order;
ptr += sizeof(int32_t);
*((double *)ptr) = msg->traj[i].start_time.toSec();
ptr += sizeof(double);
*((int64_t *)ptr) = msg->traj[i].traj_id;
ptr += sizeof(int64_t);
*((double *)ptr) = msg->traj[i].yaw_dt;
ptr += sizeof(double);
*((size_t *)ptr) = msg->traj[i].knots.size();
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->traj[i].knots.size(); j++)
{
*((double *)ptr) = msg->traj[i].knots[j];
ptr += sizeof(double);
}
*((size_t *)ptr) = msg->traj[i].pos_pts.size();
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->traj[i].pos_pts.size(); j++)
{
*((double *)ptr) = msg->traj[i].pos_pts[j].x;
ptr += sizeof(double);
*((double *)ptr) = msg->traj[i].pos_pts[j].y;
ptr += sizeof(double);
*((double *)ptr) = msg->traj[i].pos_pts[j].z;
ptr += sizeof(double);
}
*((size_t *)ptr) = msg->traj[i].yaw_pts.size();
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->traj[i].yaw_pts.size(); j++)
{
*((double *)ptr) = msg->traj[i].yaw_pts[j];
ptr += sizeof(double);
}
}
return ptr - send_buf_;
}
int serializeOdom(const nav_msgs::OdometryPtr &msg)
{
char *ptr = udp_send_buf_;
unsigned long total_len = 0;
total_len = sizeof(size_t) +
msg->child_frame_id.length() * sizeof(char) +
sizeof(size_t) +
msg->header.frame_id.length() * sizeof(char) +
sizeof(uint32_t) +
sizeof(double) +
7 * sizeof(double) +
36 * sizeof(double) +
6 * sizeof(double) +
36 * sizeof(double);
if (total_len + 1 > BUF_LEN)
{
ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN");
return -1;
}
*((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::ODOM;
ptr += sizeof(MESSAGE_TYPE);
// child_frame_id
size_t len = msg->child_frame_id.length();
*((size_t *)ptr) = len;
ptr += sizeof(size_t);
memcpy((void *)ptr, (void *)msg->child_frame_id.c_str(), len * sizeof(char));
ptr += len * sizeof(char);
// header
len = msg->header.frame_id.length();
*((size_t *)ptr) = len;
ptr += sizeof(size_t);
memcpy((void *)ptr, (void *)msg->header.frame_id.c_str(), len * sizeof(char));
ptr += len * sizeof(char);
*((uint32_t *)ptr) = msg->header.seq;
ptr += sizeof(uint32_t);
*((double *)ptr) = msg->header.stamp.toSec();
ptr += sizeof(double);
*((double *)ptr) = msg->pose.pose.position.x;
ptr += sizeof(double);
*((double *)ptr) = msg->pose.pose.position.y;
ptr += sizeof(double);
*((double *)ptr) = msg->pose.pose.position.z;
ptr += sizeof(double);
*((double *)ptr) = msg->pose.pose.orientation.w;
ptr += sizeof(double);
*((double *)ptr) = msg->pose.pose.orientation.x;
ptr += sizeof(double);
*((double *)ptr) = msg->pose.pose.orientation.y;
ptr += sizeof(double);
*((double *)ptr) = msg->pose.pose.orientation.z;
ptr += sizeof(double);
for (size_t j = 0; j < 36; j++)
{
*((double *)ptr) = msg->pose.covariance[j];
ptr += sizeof(double);
}
*((double *)ptr) = msg->twist.twist.linear.x;
ptr += sizeof(double);
*((double *)ptr) = msg->twist.twist.linear.y;
ptr += sizeof(double);
*((double *)ptr) = msg->twist.twist.linear.z;
ptr += sizeof(double);
*((double *)ptr) = msg->twist.twist.angular.x;
ptr += sizeof(double);
*((double *)ptr) = msg->twist.twist.angular.y;
ptr += sizeof(double);
*((double *)ptr) = msg->twist.twist.angular.z;
ptr += sizeof(double);
for (size_t j = 0; j < 36; j++)
{
*((double *)ptr) = msg->twist.covariance[j];
ptr += sizeof(double);
}
return ptr - udp_send_buf_;
}
int serializeStop(const std_msgs::EmptyPtr &msg)
{
char *ptr = udp_send_buf_;
*((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::STOP;
ptr += sizeof(MESSAGE_TYPE);
return ptr - udp_send_buf_;
}
int serializeOneTraj(const traj_utils::BsplinePtr &msg)
{
char *ptr = udp_send_buf_;
unsigned long total_len = 0;
total_len += sizeof(int32_t) + sizeof(int32_t) + sizeof(double) + sizeof(int64_t) + sizeof(double);
total_len += sizeof(size_t) + msg->knots.size() * sizeof(double);
total_len += sizeof(size_t) + (3 * msg->pos_pts.size()) * sizeof(double);
total_len += sizeof(size_t) + msg->yaw_pts.size() * sizeof(double);
if (total_len + 1 > BUF_LEN)
{
ROS_ERROR("[bridge_node] Topic is too large, please enlarge BUF_LEN (2)");
return -1;
}
*((MESSAGE_TYPE *)ptr) = MESSAGE_TYPE::ONE_TRAJ;
ptr += sizeof(MESSAGE_TYPE);
*((int32_t *)ptr) = msg->drone_id;
ptr += sizeof(int32_t);
*((int32_t *)ptr) = msg->order;
ptr += sizeof(int32_t);
*((double *)ptr) = msg->start_time.toSec();
ptr += sizeof(double);
*((int64_t *)ptr) = msg->traj_id;
ptr += sizeof(int64_t);
*((double *)ptr) = msg->yaw_dt;
ptr += sizeof(double);
*((size_t *)ptr) = msg->knots.size();
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->knots.size(); j++)
{
*((double *)ptr) = msg->knots[j];
ptr += sizeof(double);
}
*((size_t *)ptr) = msg->pos_pts.size();
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->pos_pts.size(); j++)
{
*((double *)ptr) = msg->pos_pts[j].x;
ptr += sizeof(double);
*((double *)ptr) = msg->pos_pts[j].y;
ptr += sizeof(double);
*((double *)ptr) = msg->pos_pts[j].z;
ptr += sizeof(double);
}
*((size_t *)ptr) = msg->yaw_pts.size();
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->yaw_pts.size(); j++)
{
*((double *)ptr) = msg->yaw_pts[j];
ptr += sizeof(double);
}
return ptr - udp_send_buf_;
}
int deserializeOneTraj(traj_utils::BsplinePtr &msg)
{
char *ptr = udp_recv_buf_;
ptr += sizeof(MESSAGE_TYPE);
msg->drone_id = *((int32_t *)ptr);
ptr += sizeof(int32_t);
msg->order = *((int32_t *)ptr);
ptr += sizeof(int32_t);
msg->start_time.fromSec(*((double *)ptr));
ptr += sizeof(double);
msg->traj_id = *((int64_t *)ptr);
ptr += sizeof(int64_t);
msg->yaw_dt = *((double *)ptr);
ptr += sizeof(double);
msg->knots.resize(*((size_t *)ptr));
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->knots.size(); j++)
{
msg->knots[j] = *((double *)ptr);
ptr += sizeof(double);
}
msg->pos_pts.resize(*((size_t *)ptr));
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->pos_pts.size(); j++)
{
msg->pos_pts[j].x = *((double *)ptr);
ptr += sizeof(double);
msg->pos_pts[j].y = *((double *)ptr);
ptr += sizeof(double);
msg->pos_pts[j].z = *((double *)ptr);
ptr += sizeof(double);
}
msg->yaw_pts.resize(*((size_t *)ptr));
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->yaw_pts.size(); j++)
{
msg->yaw_pts[j] = *((double *)ptr);
ptr += sizeof(double);
}
return ptr - udp_recv_buf_;
}
int deserializeStop(std_msgs::EmptyPtr &msg)
{
char *ptr = udp_recv_buf_;
return ptr - udp_recv_buf_;
}
int deserializeOdom(nav_msgs::OdometryPtr &msg)
{
char *ptr = udp_recv_buf_;
ptr += sizeof(MESSAGE_TYPE);
// child_frame_id
size_t len = *((size_t *)ptr);
ptr += sizeof(size_t);
msg->child_frame_id.assign((const char *)ptr, len);
ptr += len * sizeof(char);
// header
len = *((size_t *)ptr);
ptr += sizeof(size_t);
msg->header.frame_id.assign((const char *)ptr, len);
ptr += len * sizeof(char);
msg->header.seq = *((uint32_t *)ptr);
ptr += sizeof(uint32_t);
msg->header.stamp.fromSec(*((double *)ptr));
ptr += sizeof(double);
msg->pose.pose.position.x = *((double *)ptr);
ptr += sizeof(double);
msg->pose.pose.position.y = *((double *)ptr);
ptr += sizeof(double);
msg->pose.pose.position.z = *((double *)ptr);
ptr += sizeof(double);
msg->pose.pose.orientation.w = *((double *)ptr);
ptr += sizeof(double);
msg->pose.pose.orientation.x = *((double *)ptr);
ptr += sizeof(double);
msg->pose.pose.orientation.y = *((double *)ptr);
ptr += sizeof(double);
msg->pose.pose.orientation.z = *((double *)ptr);
ptr += sizeof(double);
for (size_t j = 0; j < 36; j++)
{
msg->pose.covariance[j] = *((double *)ptr);
ptr += sizeof(double);
}
msg->twist.twist.linear.x = *((double *)ptr);
ptr += sizeof(double);
msg->twist.twist.linear.y = *((double *)ptr);
ptr += sizeof(double);
msg->twist.twist.linear.z = *((double *)ptr);
ptr += sizeof(double);
msg->twist.twist.angular.x = *((double *)ptr);
ptr += sizeof(double);
msg->twist.twist.angular.y = *((double *)ptr);
ptr += sizeof(double);
msg->twist.twist.angular.z = *((double *)ptr);
ptr += sizeof(double);
for (size_t j = 0; j < 36; j++)
{
msg->twist.covariance[j] = *((double *)ptr);
ptr += sizeof(double);
}
return ptr - udp_recv_buf_;
}
int deserializeMultiBsplines(traj_utils::MultiBsplinesPtr &msg)
{
char *ptr = recv_buf_;
ptr += sizeof(MESSAGE_TYPE);
msg->drone_id_from = *((int32_t *)ptr);
ptr += sizeof(int32_t);
msg->traj.resize(*((size_t *)ptr));
ptr += sizeof(size_t);
for (size_t i = 0; i < msg->traj.size(); i++)
{
msg->traj[i].drone_id = *((int32_t *)ptr);
ptr += sizeof(int32_t);
msg->traj[i].order = *((int32_t *)ptr);
ptr += sizeof(int32_t);
msg->traj[i].start_time.fromSec(*((double *)ptr));
ptr += sizeof(double);
msg->traj[i].traj_id = *((int64_t *)ptr);
ptr += sizeof(int64_t);
msg->traj[i].yaw_dt = *((double *)ptr);
ptr += sizeof(double);
msg->traj[i].knots.resize(*((size_t *)ptr));
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->traj[i].knots.size(); j++)
{
msg->traj[i].knots[j] = *((double *)ptr);
ptr += sizeof(double);
}
msg->traj[i].pos_pts.resize(*((size_t *)ptr));
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->traj[i].pos_pts.size(); j++)
{
msg->traj[i].pos_pts[j].x = *((double *)ptr);
ptr += sizeof(double);
msg->traj[i].pos_pts[j].y = *((double *)ptr);
ptr += sizeof(double);
msg->traj[i].pos_pts[j].z = *((double *)ptr);
ptr += sizeof(double);
}
msg->traj[i].yaw_pts.resize(*((size_t *)ptr));
ptr += sizeof(size_t);
for (size_t j = 0; j < msg->traj[i].yaw_pts.size(); j++)
{
msg->traj[i].yaw_pts[j] = *((double *)ptr);
ptr += sizeof(double);
}
}
return ptr - recv_buf_;
}
void multitraj_sub_tcp_cb(const traj_utils::MultiBsplinesPtr &msg)
{
int len = serializeMultiBsplines(msg);
if (send(send_sock_, send_buf_, len, 0) <= 0)
{
ROS_ERROR("TCP SEND ERROR!!!");
}
}
void odom_sub_udp_cb(const nav_msgs::OdometryPtr &msg)
{
static ros::Time t_last;
ros::Time t_now = ros::Time::now();
if ((t_now - t_last).toSec() * odom_broadcast_freq_ < 1.0)
{
return;
}
t_last = t_now;
msg->child_frame_id = string("drone_") + std::to_string(drone_id_);
int len = serializeOdom(msg);
if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0)
{
ROS_ERROR("UDP SEND ERROR (1)!!!");
}
}
void emergency_stop_sub_udp_cb(const std_msgs::EmptyPtr &msg)
{
int len = serializeStop(msg);
if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0)
{
ROS_ERROR("UDP SEND ERROR (2)!!!");
}
}
void one_traj_sub_udp_cb(const traj_utils::BsplinePtr &msg)
{
int len = serializeOneTraj(msg);
if (sendto(udp_send_fd_, udp_send_buf_, len, 0, (struct sockaddr *)&addr_udp_send_, sizeof(addr_udp_send_)) <= 0)
{
ROS_ERROR("UDP SEND ERROR (3)!!!");
}
}
void server_fun()
{
int valread;
// Connect
if (wait_connection_from_previous_drone(PORT, server_fd_, recv_sock_) < 0)
{
ROS_ERROR("[bridge_node]Socket recever creation error!");
exit(EXIT_FAILURE);
}
while (true)
{
valread = read(recv_sock_, recv_buf_, BUF_LEN);
if ( valread <= 0 )
{
ROS_ERROR("Received message length <= 0, maybe connection has lost");
close(recv_sock_);
close(server_fd_);
return;
}
if (valread == deserializeMultiBsplines(bsplines_msg_))
{
swarm_trajs_pub_.publish(*bsplines_msg_);
}
else
{
ROS_ERROR("Received message length not matches the sent one!!!");
continue;
}
}
}
void udp_recv_fun()
{
int valread;
struct sockaddr_in addr_client;
socklen_t addr_len;
// Connect
if (udp_bind_to_port(UDP_PORT, udp_server_fd_) < 0)
{
ROS_ERROR("[bridge_node]Socket recever creation error!");
exit(EXIT_FAILURE);
}
while (true)
{
if ((valread = recvfrom(udp_server_fd_, udp_recv_buf_, BUF_LEN, 0, (struct sockaddr *)&addr_client, (socklen_t *)&addr_len)) < 0)
{
perror("recvfrom error:");
exit(EXIT_FAILURE);
}
char *ptr = udp_recv_buf_;
switch (*((MESSAGE_TYPE *)ptr))
{
case MESSAGE_TYPE::STOP:
{
if (valread == sizeof(std_msgs::Empty))
{
if (valread == deserializeStop(stop_msg_))
{
emergency_stop_pub_.publish(*stop_msg_);
}
else
{
ROS_ERROR("Received message length not matches the sent one (1)!!!");
continue;
}
}
break;
}
case MESSAGE_TYPE::ODOM:
{
if (valread == deserializeOdom(odom_msg_))
{
other_odoms_pub_.publish(*odom_msg_);
}
else
{
ROS_ERROR("Received message length not matches the sent one (2)!!!");
continue;
}
break;
}
case MESSAGE_TYPE::ONE_TRAJ:
{
if ( valread == deserializeOneTraj(bspline_msg_) )
{
one_traj_pub_.publish(*bspline_msg_);
}
else
{
ROS_ERROR("Received message length not matches the sent one (3)!!!");
continue;
}
break;
}
default:
//ROS_ERROR("Unknown received message???");
break;
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosmsg_tcp_bridge");
ros::NodeHandle nh("~");
nh.param("next_drone_ip", tcp_ip_, string("127.0.0.1"));
nh.param("broadcast_ip", udp_ip_, string("127.0.0.255"));
nh.param("drone_id", drone_id_, -1);
nh.param("odom_max_freq", odom_broadcast_freq_, 1000.0);
bsplines_msg_.reset(new traj_utils::MultiBsplines);
odom_msg_.reset(new nav_msgs::Odometry);
stop_msg_.reset(new std_msgs::Empty);
bspline_msg_.reset(new traj_utils::Bspline);
if (drone_id_ == -1)
{
ROS_ERROR("Wrong drone_id!");
exit(EXIT_FAILURE);
}
string sub_traj_topic_name = string("/uav") + std::to_string(drone_id_) + string("_planning/swarm_trajs");
swarm_trajs_sub_ = nh.subscribe(sub_traj_topic_name.c_str(), 10, multitraj_sub_tcp_cb, ros::TransportHints().tcpNoDelay());
if ( drone_id_ >= 2 )
{
string pub_traj_topic_name = string("/uav") + std::to_string(drone_id_ - 1) + string("_planning/swarm_trajs");
swarm_trajs_pub_ = nh.advertise<traj_utils::MultiBsplines>(pub_traj_topic_name.c_str(), 10);
}
// other_odoms_sub_ = nh.subscribe("my_odom", 10, odom_sub_udp_cb, ros::TransportHints().tcpNoDelay());
// other_odoms_pub_ = nh.advertise<nav_msgs::Odometry>("/others_odom", 10);
//emergency_stop_sub_ = nh.subscribe("emergency_stop_broadcast", 10, emergency_stop_sub_udp_cb, ros::TransportHints().tcpNoDelay());
//emergency_stop_pub_ = nh.advertise<std_msgs::Empty>("emergency_stop_recv", 10);
one_traj_sub_ = nh.subscribe("/broadcast_bspline", 100, one_traj_sub_udp_cb, ros::TransportHints().tcpNoDelay());
one_traj_pub_ = nh.advertise<traj_utils::Bspline>("/broadcast_bspline2", 100);
boost::thread recv_thd(server_fun);
recv_thd.detach();
ros::Duration(0.1).sleep();
boost::thread udp_recv_thd(udp_recv_fun);
udp_recv_thd.detach();
ros::Duration(0.1).sleep();
// TCP connect
send_sock_ = connect_to_next_drone(tcp_ip_.c_str(), PORT);
// UDP connect
udp_send_fd_ = init_broadcast(udp_ip_.c_str(), UDP_PORT);
cout << "[rosmsg_tcp_bridge] start running" << endl;
ros::spin();
close(send_sock_);
close(recv_sock_);
close(server_fd_);
close(udp_server_fd_);
close(udp_send_fd_);
return 0;
}

59
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/traj_utils/CMakeLists.txt
Unescape View File

@ -1,59 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(traj_utils)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
find_package(catkin REQUIRED COMPONENTS
#bspline_opt
#path_searching
roscpp
std_msgs
geometry_msgs
message_generation
#cv_bridge
)
find_package(Eigen3 REQUIRED)
find_package(PCL 1.7 REQUIRED)
# Generate messages in the 'msg' folder
add_message_files(
FILES
Bspline.msg
DataDisp.msg
MultiBsplines.msg
)
# Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
geometry_msgs
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES traj_utils
CATKIN_DEPENDS message_runtime
# DEPENDS system_lib
)
include_directories(
SYSTEM
include
${catkin_INCLUDE_DIRS}
${Eigen3_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
link_directories(${PCL_LIBRARY_DIRS})
add_library( traj_utils
src/planning_visualization.cpp
src/polynomial_traj.cpp
)
target_link_libraries( traj_utils
${catkin_LIBRARIES}
)

233
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/traj_utils/include/traj_utils/plan_container.hpp
Unescape View File

@ -1,233 +0,0 @@
#ifndef _PLAN_CONTAINER_H_
#define _PLAN_CONTAINER_H_
#include <Eigen/Eigen>
#include <vector>
#include <ros/ros.h>
#include <bspline_opt/uniform_bspline.h>
#include <traj_utils/polynomial_traj.h>
using std::vector;
namespace ego_planner
{
class GlobalTrajData
{
private:
public:
PolynomialTraj global_traj_;
vector<UniformBspline> local_traj_;
double global_duration_;
ros::Time global_start_time_;
double local_start_time_, local_end_time_;
double time_increase_;
double last_time_inc_;
double last_progress_time_;
GlobalTrajData(/* args */) {}
~GlobalTrajData() {}
bool localTrajReachTarget() { return fabs(local_end_time_ - global_duration_) < 0.1; }
void setGlobalTraj(const PolynomialTraj &traj, const ros::Time &time)
{
global_traj_ = traj;
global_traj_.init();
global_duration_ = global_traj_.getTimeSum();
global_start_time_ = time;
local_traj_.clear();
local_start_time_ = -1;
local_end_time_ = -1;
time_increase_ = 0.0;
last_time_inc_ = 0.0;
last_progress_time_ = 0.0;
}
void setLocalTraj(UniformBspline traj, double local_ts, double local_te, double time_inc)
{
local_traj_.resize(3);
local_traj_[0] = traj;
local_traj_[1] = local_traj_[0].getDerivative();
local_traj_[2] = local_traj_[1].getDerivative();
local_start_time_ = local_ts;
local_end_time_ = local_te;
global_duration_ += time_inc;
time_increase_ += time_inc;
last_time_inc_ = time_inc;
}
Eigen::Vector3d getPosition(double t)
{
if (t >= -1e-3 && t <= local_start_time_)
{
return global_traj_.evaluate(t - time_increase_ + last_time_inc_);
}
else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
{
return global_traj_.evaluate(t - time_increase_);
}
else
{
double tm, tmp;
local_traj_[0].getTimeSpan(tm, tmp);
return local_traj_[0].evaluateDeBoor(tm + t - local_start_time_);
}
}
Eigen::Vector3d getVelocity(double t)
{
if (t >= -1e-3 && t <= local_start_time_)
{
return global_traj_.evaluateVel(t);
}
else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
{
return global_traj_.evaluateVel(t - time_increase_);
}
else
{
double tm, tmp;
local_traj_[0].getTimeSpan(tm, tmp);
return local_traj_[1].evaluateDeBoor(tm + t - local_start_time_);
}
}
Eigen::Vector3d getAcceleration(double t)
{
if (t >= -1e-3 && t <= local_start_time_)
{
return global_traj_.evaluateAcc(t);
}
else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
{
return global_traj_.evaluateAcc(t - time_increase_);
}
else
{
double tm, tmp;
local_traj_[0].getTimeSpan(tm, tmp);
return local_traj_[2].evaluateDeBoor(tm + t - local_start_time_);
}
}
// get Bspline paramterization data of a local trajectory within a sphere
// start_t: start time of the trajectory
// dist_pt: distance between the discretized points
void getTrajByRadius(const double &start_t, const double &des_radius, const double &dist_pt,
vector<Eigen::Vector3d> &point_set, vector<Eigen::Vector3d> &start_end_derivative,
double &dt, double &seg_duration)
{
double seg_length = 0.0; // length of the truncated segment
double seg_time = 0.0; // duration of the truncated segment
double radius = 0.0; // distance to the first point of the segment
double delt = 0.2;
Eigen::Vector3d first_pt = getPosition(start_t); // first point of the segment
Eigen::Vector3d prev_pt = first_pt; // previous point
Eigen::Vector3d cur_pt; // current point
// go forward until the traj exceed radius or global time
while (radius < des_radius && seg_time < global_duration_ - start_t - 1e-3)
{
seg_time += delt;
seg_time = min(seg_time, global_duration_ - start_t);
cur_pt = getPosition(start_t + seg_time);
seg_length += (cur_pt - prev_pt).norm();
prev_pt = cur_pt;
radius = (cur_pt - first_pt).norm();
}
// get parameterization dt by desired density of points
int seg_num = floor(seg_length / dist_pt);
// get outputs
seg_duration = seg_time; // duration of the truncated segment
dt = seg_time / seg_num; // time difference between two points
for (double tp = 0.0; tp <= seg_time + 1e-4; tp += dt)
{
cur_pt = getPosition(start_t + tp);
point_set.push_back(cur_pt);
}
start_end_derivative.push_back(getVelocity(start_t));
start_end_derivative.push_back(getVelocity(start_t + seg_time));
start_end_derivative.push_back(getAcceleration(start_t));
start_end_derivative.push_back(getAcceleration(start_t + seg_time));
}
// get Bspline paramterization data of a fixed duration local trajectory
// start_t: start time of the trajectory
// duration: time length of the segment
// seg_num: discretized the segment into *seg_num* parts
void getTrajByDuration(double start_t, double duration, int seg_num,
vector<Eigen::Vector3d> &point_set,
vector<Eigen::Vector3d> &start_end_derivative, double &dt)
{
dt = duration / seg_num;
Eigen::Vector3d cur_pt;
for (double tp = 0.0; tp <= duration + 1e-4; tp += dt)
{
cur_pt = getPosition(start_t + tp);
point_set.push_back(cur_pt);
}
start_end_derivative.push_back(getVelocity(start_t));
start_end_derivative.push_back(getVelocity(start_t + duration));
start_end_derivative.push_back(getAcceleration(start_t));
start_end_derivative.push_back(getAcceleration(start_t + duration));
}
};
struct PlanParameters
{
/* planning algorithm parameters */
double max_vel_, max_acc_, max_jerk_; // physical limits
double ctrl_pt_dist; // distance between adjacient B-spline control points
double feasibility_tolerance_; // permitted ratio of vel/acc exceeding limits
double planning_horizen_;
bool use_distinctive_trajs;
int drone_id; // single drone: drone_id <= -1, swarm: drone_id >= 0
/* processing time */
double time_search_ = 0.0;
double time_optimize_ = 0.0;
double time_adjust_ = 0.0;
};
struct LocalTrajData
{
/* info of generated traj */
int traj_id_;
double duration_;
ros::Time start_time_;
Eigen::Vector3d start_pos_;
UniformBspline position_traj_, velocity_traj_, acceleration_traj_;
};
struct OneTrajDataOfSwarm
{
/* info of generated traj */
int drone_id;
double duration_;
ros::Time start_time_;
Eigen::Vector3d start_pos_;
UniformBspline position_traj_;
};
typedef std::vector<OneTrajDataOfSwarm> SwarmTrajData;
} // namespace ego_planner
#endif

55
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/traj_utils/include/traj_utils/planning_visualization.h
Unescape View File

@ -1,55 +0,0 @@
#ifndef _PLANNING_VISUALIZATION_H_
#define _PLANNING_VISUALIZATION_H_
#include <Eigen/Eigen>
#include <algorithm>
//#include <bspline_opt/uniform_bspline.h>
#include <iostream>
//#include <bspline_opt/polynomial_traj.h>
#include <ros/ros.h>
#include <vector>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <stdlib.h>
using std::vector;
namespace ego_planner
{
class PlanningVisualization
{
private:
ros::NodeHandle node;
ros::Publisher goal_point_pub;
ros::Publisher global_list_pub;
ros::Publisher init_list_pub;
ros::Publisher optimal_list_pub;
ros::Publisher a_star_list_pub;
ros::Publisher guide_vector_pub;
ros::Publisher intermediate_state_pub;
public:
PlanningVisualization(/* args */) {}
~PlanningVisualization() {}
PlanningVisualization(ros::NodeHandle &nh);
typedef std::shared_ptr<PlanningVisualization> Ptr;
void displayMarkerList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale,
Eigen::Vector4d color, int id, bool show_sphere = true);
void generatePathDisplayArray(visualization_msgs::MarkerArray &array,
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
void generateArrowDisplayArray(visualization_msgs::MarkerArray &array,
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
void displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id);
void displayGlobalPathList(vector<Eigen::Vector3d> global_pts, const double scale, int id);
void displayInitPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id);
void displayMultiInitPathList(vector<vector<Eigen::Vector3d>> init_trajs, const double scale);
void displayOptimalList(Eigen::MatrixXd optimal_pts, int id);
void displayAStarList(std::vector<std::vector<Eigen::Vector3d>> a_star_paths, int id);
void displayArrowList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id);
// void displayIntermediateState(ros::Publisher& intermediate_pub, ego_planner::BsplineOptimizer::Ptr optimizer, double sleep_time, const int start_iteration);
// void displayNewArrow(ros::Publisher& guide_vector_pub, ego_planner::BsplineOptimizer::Ptr optimizer);
};
} // namespace ego_planner
#endif

336
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/traj_utils/include/traj_utils/polynomial_traj.h
Unescape View File

@ -1,336 +0,0 @@
#ifndef _POLYNOMIAL_TRAJ_H
#define _POLYNOMIAL_TRAJ_H
#include <Eigen/Eigen>
#include <vector>
using std::vector;
class PolynomialTraj
{
private:
vector<double> times; // time of each segment
vector<vector<double>> cxs; // coefficient of x of each segment, from n-1 -> 0
vector<vector<double>> cys; // coefficient of y of each segment
vector<vector<double>> czs; // coefficient of z of each segment
double time_sum;
int num_seg;
/* evaluation */
vector<Eigen::Vector3d> traj_vec3d;
double length;
public:
PolynomialTraj(/* args */)
{
}
~PolynomialTraj()
{
}
void reset()
{
times.clear(), cxs.clear(), cys.clear(), czs.clear();
time_sum = 0.0, num_seg = 0;
}
void addSegment(vector<double> cx, vector<double> cy, vector<double> cz, double t)
{
cxs.push_back(cx), cys.push_back(cy), czs.push_back(cz), times.push_back(t);
}
void init()
{
num_seg = times.size();
time_sum = 0.0;
for (int i = 0; i < times.size(); ++i)
{
time_sum += times[i];
}
}
vector<double> getTimes()
{
return times;
}
vector<vector<double>> getCoef(int axis)
{
switch (axis)
{
case 0:
return cxs;
case 1:
return cys;
case 2:
return czs;
default:
std::cout << "\033[31mIllegal axis!\033[0m" << std::endl;
}
vector<vector<double>> empty;
return empty;
}
Eigen::Vector3d evaluate(double t)
{
/* detetrmine segment num */
int idx = 0;
while (times[idx] + 1e-4 < t)
{
t -= times[idx];
++idx;
}
/* evaluation */
int order = cxs[idx].size();
Eigen::VectorXd cx(order), cy(order), cz(order), tv(order);
for (int i = 0; i < order; ++i)
{
cx(i) = cxs[idx][i], cy(i) = cys[idx][i], cz(i) = czs[idx][i];
tv(order - 1 - i) = std::pow(t, double(i));
}
Eigen::Vector3d pt;
pt(0) = tv.dot(cx), pt(1) = tv.dot(cy), pt(2) = tv.dot(cz);
return pt;
}
Eigen::Vector3d evaluateVel(double t)
{
/* detetrmine segment num */
int idx = 0;
while (times[idx] + 1e-4 < t)
{
t -= times[idx];
++idx;
}
/* evaluation */
int order = cxs[idx].size();
Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1);
/* coef of vel */
for (int i = 0; i < order - 1; ++i)
{
vx(i) = double(i + 1) * cxs[idx][order - 2 - i];
vy(i) = double(i + 1) * cys[idx][order - 2 - i];
vz(i) = double(i + 1) * czs[idx][order - 2 - i];
}
double ts = t;
Eigen::VectorXd tv(order - 1);
for (int i = 0; i < order - 1; ++i)
tv(i) = pow(ts, i);
Eigen::Vector3d vel;
vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz);
return vel;
}
Eigen::Vector3d evaluateAcc(double t)
{
/* detetrmine segment num */
int idx = 0;
while (times[idx] + 1e-4 < t)
{
t -= times[idx];
++idx;
}
/* evaluation */
int order = cxs[idx].size();
Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2);
/* coef of vel */
for (int i = 0; i < order - 2; ++i)
{
ax(i) = double((i + 2) * (i + 1)) * cxs[idx][order - 3 - i];
ay(i) = double((i + 2) * (i + 1)) * cys[idx][order - 3 - i];
az(i) = double((i + 2) * (i + 1)) * czs[idx][order - 3 - i];
}
double ts = t;
Eigen::VectorXd tv(order - 2);
for (int i = 0; i < order - 2; ++i)
tv(i) = pow(ts, i);
Eigen::Vector3d acc;
acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az);
return acc;
}
/* for evaluating traj, should be called in sequence!!! */
double getTimeSum()
{
return this->time_sum;
}
vector<Eigen::Vector3d> getTraj()
{
double eval_t = 0.0;
traj_vec3d.clear();
while (eval_t < time_sum)
{
Eigen::Vector3d pt = evaluate(eval_t);
traj_vec3d.push_back(pt);
eval_t += 0.01;
}
return traj_vec3d;
}
double getLength()
{
length = 0.0;
Eigen::Vector3d p_l = traj_vec3d[0], p_n;
for (int i = 1; i < traj_vec3d.size(); ++i)
{
p_n = traj_vec3d[i];
length += (p_n - p_l).norm();
p_l = p_n;
}
return length;
}
double getMeanVel()
{
double mean_vel = length / time_sum;
}
double getAccCost()
{
double cost = 0.0;
int order = cxs[0].size();
for (int s = 0; s < times.size(); ++s)
{
Eigen::Vector3d um;
um(0) = 2 * cxs[s][order - 3], um(1) = 2 * cys[s][order - 3], um(2) = 2 * czs[s][order - 3];
cost += um.squaredNorm() * times[s];
}
return cost;
}
double getJerk()
{
double jerk = 0.0;
/* evaluate jerk */
for (int s = 0; s < times.size(); ++s)
{
Eigen::VectorXd cxv(cxs[s].size()), cyv(cys[s].size()), czv(czs[s].size());
/* convert coefficient */
int order = cxs[s].size();
for (int j = 0; j < order; ++j)
{
cxv(j) = cxs[s][order - 1 - j], cyv(j) = cys[s][order - 1 - j], czv(j) = czs[s][order - 1 - j];
}
double ts = times[s];
/* jerk matrix */
Eigen::MatrixXd mat_jerk(order, order);
mat_jerk.setZero();
for (double i = 3; i < order; i += 1)
for (double j = 3; j < order; j += 1)
{
mat_jerk(i, j) =
i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) * pow(ts, i + j - 5) / (i + j - 5);
}
jerk += (cxv.transpose() * mat_jerk * cxv)(0, 0);
jerk += (cyv.transpose() * mat_jerk * cyv)(0, 0);
jerk += (czv.transpose() * mat_jerk * czv)(0, 0);
}
return jerk;
}
void getMeanAndMaxVel(double &mean_v, double &max_v)
{
int num = 0;
mean_v = 0.0, max_v = -1.0;
for (int s = 0; s < times.size(); ++s)
{
int order = cxs[s].size();
Eigen::VectorXd vx(order - 1), vy(order - 1), vz(order - 1);
/* coef of vel */
for (int i = 0; i < order - 1; ++i)
{
vx(i) = double(i + 1) * cxs[s][order - 2 - i];
vy(i) = double(i + 1) * cys[s][order - 2 - i];
vz(i) = double(i + 1) * czs[s][order - 2 - i];
}
double ts = times[s];
double eval_t = 0.0;
while (eval_t < ts)
{
Eigen::VectorXd tv(order - 1);
for (int i = 0; i < order - 1; ++i)
tv(i) = pow(ts, i);
Eigen::Vector3d vel;
vel(0) = tv.dot(vx), vel(1) = tv.dot(vy), vel(2) = tv.dot(vz);
double vn = vel.norm();
mean_v += vn;
if (vn > max_v)
max_v = vn;
++num;
eval_t += 0.01;
}
}
mean_v = mean_v / double(num);
}
void getMeanAndMaxAcc(double &mean_a, double &max_a)
{
int num = 0;
mean_a = 0.0, max_a = -1.0;
for (int s = 0; s < times.size(); ++s)
{
int order = cxs[s].size();
Eigen::VectorXd ax(order - 2), ay(order - 2), az(order - 2);
/* coef of acc */
for (int i = 0; i < order - 2; ++i)
{
ax(i) = double((i + 2) * (i + 1)) * cxs[s][order - 3 - i];
ay(i) = double((i + 2) * (i + 1)) * cys[s][order - 3 - i];
az(i) = double((i + 2) * (i + 1)) * czs[s][order - 3 - i];
}
double ts = times[s];
double eval_t = 0.0;
while (eval_t < ts)
{
Eigen::VectorXd tv(order - 2);
for (int i = 0; i < order - 2; ++i)
tv(i) = pow(ts, i);
Eigen::Vector3d acc;
acc(0) = tv.dot(ax), acc(1) = tv.dot(ay), acc(2) = tv.dot(az);
double an = acc.norm();
mean_a += an;
if (an > max_a)
max_a = an;
++num;
eval_t += 0.01;
}
}
mean_a = mean_a / double(num);
}
static PolynomialTraj minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel,
const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc,
const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time);
static PolynomialTraj one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc,
double t);
};
#endif

12
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/traj_utils/msg/Bspline.msg
Unescape View File

@ -1,12 +0,0 @@
int32 drone_id
int32 order
int64 traj_id
time start_time
float64[] knots
geometry_msgs/Point[] pos_pts
float64[] yaw_pts
float64 yaw_dt

7
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/traj_utils/msg/DataDisp.msg
Unescape View File

@ -1,7 +0,0 @@
Header header
float64 a
float64 b
float64 c
float64 d
float64 e

4
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/traj_utils/msg/MultiBsplines.msg
Unescape View File

@ -1,4 +0,0 @@
int32 drone_id_from
Bspline[] traj

77
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/traj_utils/package.xml
Unescape View File

@ -1,77 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>traj_utils</name>
<version>0.0.0</version>
<description>The traj_utils package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="bzhouai@todo.todo">bzhouai</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/traj_utils</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<!--build_depend>bspline_opt</build_depend-->
<!--build_depend>path_searching</build_depend-->
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>catkin</build_export_depend>
<!-- <build_export_depend>bspline_opt</build_export_depend> -->
<!--build_export_depend>path_searching</build_export_depend-->
<!--build_export_depend>roscpp</build_export_depend-->
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>catkin</exec_depend>
<!--exec_depend>bspline_opt</exec_depend-->
<!--exec_depend>path_searching</exec_depend-->
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

265
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/traj_utils/src/planning_visualization.cpp
Unescape View File

@ -1,265 +0,0 @@
#include <traj_utils/planning_visualization.h>
using std::cout;
using std::endl;
namespace ego_planner
{
PlanningVisualization::PlanningVisualization(ros::NodeHandle &nh)
{
node = nh;
goal_point_pub = nh.advertise<visualization_msgs::Marker>("goal_point", 2);
global_list_pub = nh.advertise<visualization_msgs::Marker>("global_list", 2);
init_list_pub = nh.advertise<visualization_msgs::Marker>("init_list", 2);
optimal_list_pub = nh.advertise<visualization_msgs::Marker>("optimal_list", 2);
a_star_list_pub = nh.advertise<visualization_msgs::Marker>("a_star_list", 20);
}
// // real ids used: {id, id+1000}
void PlanningVisualization::displayMarkerList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale,
Eigen::Vector4d color, int id, bool show_sphere /* = true */ )
{
visualization_msgs::Marker sphere, line_strip;
sphere.header.frame_id = line_strip.header.frame_id = "world";
sphere.header.stamp = line_strip.header.stamp = ros::Time::now();
sphere.type = visualization_msgs::Marker::SPHERE_LIST;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
sphere.action = line_strip.action = visualization_msgs::Marker::ADD;
sphere.id = id;
line_strip.id = id + 1000;
sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
sphere.color.r = line_strip.color.r = color(0);
sphere.color.g = line_strip.color.g = color(1);
sphere.color.b = line_strip.color.b = color(2);
sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0;
sphere.scale.x = scale;
sphere.scale.y = scale;
sphere.scale.z = scale;
line_strip.scale.x = scale / 2;
geometry_msgs::Point pt;
for (int i = 0; i < int(list.size()); i++)
{
pt.x = list[i](0);
pt.y = list[i](1);
pt.z = list[i](2);
//if (show_sphere) sphere.points.push_back(pt);
line_strip.points.push_back(pt);
}
//if (show_sphere) pub.publish(sphere);
pub.publish(line_strip);
}
// real ids used: {id, id+1}
void PlanningVisualization::generatePathDisplayArray(visualization_msgs::MarkerArray &array,
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
{
visualization_msgs::Marker sphere, line_strip;
sphere.header.frame_id = line_strip.header.frame_id = "map";
sphere.header.stamp = line_strip.header.stamp = ros::Time::now();
sphere.type = visualization_msgs::Marker::SPHERE_LIST;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
sphere.action = line_strip.action = visualization_msgs::Marker::ADD;
sphere.id = id;
line_strip.id = id + 1;
sphere.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
sphere.color.r = line_strip.color.r = color(0);
sphere.color.g = line_strip.color.g = color(1);
sphere.color.b = line_strip.color.b = color(2);
sphere.color.a = line_strip.color.a = color(3) > 1e-5 ? color(3) : 1.0;
sphere.scale.x = scale;
sphere.scale.y = scale;
sphere.scale.z = scale;
line_strip.scale.x = scale / 3;
geometry_msgs::Point pt;
for (int i = 0; i < int(list.size()); i++)
{
pt.x = list[i](0);
pt.y = list[i](1);
pt.z = list[i](2);
sphere.points.push_back(pt);
line_strip.points.push_back(pt);
}
array.markers.push_back(sphere);
array.markers.push_back(line_strip);
}
// real ids used: {1000*id ~ (arrow nums)+1000*id}
void PlanningVisualization::generateArrowDisplayArray(visualization_msgs::MarkerArray &array,
const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
{
visualization_msgs::Marker arrow;
arrow.header.frame_id = "map";
arrow.header.stamp = ros::Time::now();
arrow.type = visualization_msgs::Marker::ARROW;
arrow.action = visualization_msgs::Marker::ADD;
// geometry_msgs::Point start, end;
// arrow.points
arrow.color.r = color(0);
arrow.color.g = color(1);
arrow.color.b = color(2);
arrow.color.a = color(3) > 1e-5 ? color(3) : 1.0;
arrow.scale.x = scale;
arrow.scale.y = 2 * scale;
arrow.scale.z = 2 * scale;
geometry_msgs::Point start, end;
for (int i = 0; i < int(list.size() / 2); i++)
{
// arrow.color.r = color(0) / (1+i);
// arrow.color.g = color(1) / (1+i);
// arrow.color.b = color(2) / (1+i);
start.x = list[2 * i](0);
start.y = list[2 * i](1);
start.z = list[2 * i](2);
end.x = list[2 * i + 1](0);
end.y = list[2 * i + 1](1);
end.z = list[2 * i + 1](2);
arrow.points.clear();
arrow.points.push_back(start);
arrow.points.push_back(end);
arrow.id = i + id * 1000;
array.markers.push_back(arrow);
}
}
void PlanningVisualization::displayGoalPoint(Eigen::Vector3d goal_point, Eigen::Vector4d color, const double scale, int id)
{
visualization_msgs::Marker sphere;
sphere.header.frame_id = "world";
sphere.header.stamp = ros::Time::now();
sphere.type = visualization_msgs::Marker::SPHERE;
sphere.action = visualization_msgs::Marker::ADD;
sphere.id = id;
sphere.pose.orientation.w = 1.0;
sphere.color.r = color(0);
sphere.color.g = color(1);
sphere.color.b = color(2);
sphere.color.a = color(3);
sphere.scale.x = scale;
sphere.scale.y = scale;
sphere.scale.z = scale;
sphere.pose.position.x = goal_point(0);
sphere.pose.position.y = goal_point(1);
sphere.pose.position.z = goal_point(2);
goal_point_pub.publish(sphere);
}
void PlanningVisualization::displayGlobalPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id)
{
if (global_list_pub.getNumSubscribers() == 0)
{
return;
}
Eigen::Vector4d color(0, 0.5, 0.5, 1);
displayMarkerList(global_list_pub, init_pts, scale, color, id);
}
void PlanningVisualization::displayMultiInitPathList(vector<vector<Eigen::Vector3d>> init_trajs, const double scale)
{
if (init_list_pub.getNumSubscribers() == 0)
{
return;
}
static int last_nums = 0;
for ( int id=0; id<last_nums; id++ )
{
Eigen::Vector4d color(0, 0, 0, 0);
vector<Eigen::Vector3d> blank;
displayMarkerList(init_list_pub, blank, scale, color, id, false);
ros::Duration(0.001).sleep();
}
last_nums = 0;
for ( int id=0; id<init_trajs.size(); id++ )
{
Eigen::Vector4d color(0, 0, 1, 0.7);
displayMarkerList(init_list_pub, init_trajs[id], scale, color, id, false);
ros::Duration(0.001).sleep();
last_nums++;
}
}
void PlanningVisualization::displayInitPathList(vector<Eigen::Vector3d> init_pts, const double scale, int id)
{
if (init_list_pub.getNumSubscribers() == 0)
{
return;
}
Eigen::Vector4d color(0, 0, 1, 1);
displayMarkerList(init_list_pub, init_pts, scale, color, id);
}
void PlanningVisualization::displayOptimalList(Eigen::MatrixXd optimal_pts, int id)
{
if (optimal_list_pub.getNumSubscribers() == 0)
{
return;
}
vector<Eigen::Vector3d> list;
for (int i = 0; i < optimal_pts.cols(); i++)
{
Eigen::Vector3d pt = optimal_pts.col(i).transpose();
list.push_back(pt);
}
Eigen::Vector4d color(1, 0, 0, 1);
displayMarkerList(optimal_list_pub, list, 0.15, color, id);
}
void PlanningVisualization::displayAStarList(std::vector<std::vector<Eigen::Vector3d>> a_star_paths, int id /* = Eigen::Vector4d(0.5,0.5,0,1)*/)
{
if (a_star_list_pub.getNumSubscribers() == 0)
{
return;
}
int i = 0;
vector<Eigen::Vector3d> list;
Eigen::Vector4d color = Eigen::Vector4d(0.5 + ((double)rand() / RAND_MAX / 2), 0.5 + ((double)rand() / RAND_MAX / 2), 0, 1); // make the A star pathes different every time.
double scale = 0.05 + (double)rand() / RAND_MAX / 10;
for (auto block : a_star_paths)
{
list.clear();
for (auto pt : block)
{
list.push_back(pt);
}
//Eigen::Vector4d color(0.5,0.5,0,1);
displayMarkerList(a_star_list_pub, list, scale, color, id + i); // real ids used: [ id ~ id+a_star_paths.size() ]
i++;
}
}
void PlanningVisualization::displayArrowList(ros::Publisher &pub, const vector<Eigen::Vector3d> &list, double scale, Eigen::Vector4d color, int id)
{
visualization_msgs::MarkerArray array;
// clear
pub.publish(array);
generateArrowDisplayArray(array, list, scale, color, id);
pub.publish(array);
}
// PlanningVisualization::
} // namespace ego_planner

224
src/UAV/Modules/ego_planner/ego-planner-swarm/src/planner/traj_utils/src/polynomial_traj.cpp
Unescape View File

@ -1,224 +0,0 @@
#include <iostream>
#include <traj_utils/polynomial_traj.h>
PolynomialTraj PolynomialTraj::minSnapTraj(const Eigen::MatrixXd &Pos, const Eigen::Vector3d &start_vel,
const Eigen::Vector3d &end_vel, const Eigen::Vector3d &start_acc,
const Eigen::Vector3d &end_acc, const Eigen::VectorXd &Time)
{
int seg_num = Time.size();
Eigen::MatrixXd poly_coeff(seg_num, 3 * 6);
Eigen::VectorXd Px(6 * seg_num), Py(6 * seg_num), Pz(6 * seg_num);
int num_f, num_p; // number of fixed and free variables
int num_d; // number of all segments' derivatives
const static auto Factorial = [](int x) {
int fac = 1;
for (int i = x; i > 0; i--)
fac = fac * i;
return fac;
};
/* ---------- end point derivative ---------- */
Eigen::VectorXd Dx = Eigen::VectorXd::Zero(seg_num * 6);
Eigen::VectorXd Dy = Eigen::VectorXd::Zero(seg_num * 6);
Eigen::VectorXd Dz = Eigen::VectorXd::Zero(seg_num * 6);
for (int k = 0; k < seg_num; k++)
{
/* position to derivative */
Dx(k * 6) = Pos(0, k);
Dx(k * 6 + 1) = Pos(0, k + 1);
Dy(k * 6) = Pos(1, k);
Dy(k * 6 + 1) = Pos(1, k + 1);
Dz(k * 6) = Pos(2, k);
Dz(k * 6 + 1) = Pos(2, k + 1);
if (k == 0)
{
Dx(k * 6 + 2) = start_vel(0);
Dy(k * 6 + 2) = start_vel(1);
Dz(k * 6 + 2) = start_vel(2);
Dx(k * 6 + 4) = start_acc(0);
Dy(k * 6 + 4) = start_acc(1);
Dz(k * 6 + 4) = start_acc(2);
}
else if (k == seg_num - 1)
{
Dx(k * 6 + 3) = end_vel(0);
Dy(k * 6 + 3) = end_vel(1);
Dz(k * 6 + 3) = end_vel(2);
Dx(k * 6 + 5) = end_acc(0);
Dy(k * 6 + 5) = end_acc(1);
Dz(k * 6 + 5) = end_acc(2);
}
}
/* ---------- Mapping Matrix A ---------- */
Eigen::MatrixXd Ab;
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);
for (int k = 0; k < seg_num; k++)
{
Ab = Eigen::MatrixXd::Zero(6, 6);
for (int i = 0; i < 3; i++)
{
Ab(2 * i, i) = Factorial(i);
for (int j = i; j < 6; j++)
Ab(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time(k), j - i);
}
A.block(k * 6, k * 6, 6, 6) = Ab;
}
/* ---------- Produce Selection Matrix C' ---------- */
Eigen::MatrixXd Ct, C;
num_f = 2 * seg_num + 4; // 3 + 3 + (seg_num - 1) * 2 = 2m + 4
num_p = 2 * seg_num - 2; //(seg_num - 1) * 2 = 2m - 2
num_d = 6 * seg_num;
Ct = Eigen::MatrixXd::Zero(num_d, num_f + num_p);
Ct(0, 0) = 1;
Ct(2, 1) = 1;
Ct(4, 2) = 1; // stack the start point
Ct(1, 3) = 1;
Ct(3, 2 * seg_num + 4) = 1;
Ct(5, 2 * seg_num + 5) = 1;
Ct(6 * (seg_num - 1) + 0, 2 * seg_num + 0) = 1;
Ct(6 * (seg_num - 1) + 1, 2 * seg_num + 1) = 1; // Stack the end point
Ct(6 * (seg_num - 1) + 2, 4 * seg_num + 0) = 1;
Ct(6 * (seg_num - 1) + 3, 2 * seg_num + 2) = 1; // Stack the end point
Ct(6 * (seg_num - 1) + 4, 4 * seg_num + 1) = 1;
Ct(6 * (seg_num - 1) + 5, 2 * seg_num + 3) = 1; // Stack the end point
for (int j = 2; j < seg_num; j++)
{
Ct(6 * (j - 1) + 0, 2 + 2 * (j - 1) + 0) = 1;
Ct(6 * (j - 1) + 1, 2 + 2 * (j - 1) + 1) = 1;
Ct(6 * (j - 1) + 2, 2 * seg_num + 4 + 2 * (j - 2) + 0) = 1;
Ct(6 * (j - 1) + 3, 2 * seg_num + 4 + 2 * (j - 1) + 0) = 1;
Ct(6 * (j - 1) + 4, 2 * seg_num + 4 + 2 * (j - 2) + 1) = 1;
Ct(6 * (j - 1) + 5, 2 * seg_num + 4 + 2 * (j - 1) + 1) = 1;
}
C = Ct.transpose();
Eigen::VectorXd Dx1 = C * Dx;
Eigen::VectorXd Dy1 = C * Dy;
Eigen::VectorXd Dz1 = C * Dz;
/* ---------- minimum snap matrix ---------- */
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);
for (int k = 0; k < seg_num; k++)
{
for (int i = 3; i < 6; i++)
{
for (int j = 3; j < 6; j++)
{
Q(k * 6 + i, k * 6 + j) =
i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * pow(Time(k), (i + j - 5));
}
}
}
/* ---------- R matrix ---------- */
Eigen::MatrixXd R = C * A.transpose().inverse() * Q * A.inverse() * Ct;
Eigen::VectorXd Dxf(2 * seg_num + 4), Dyf(2 * seg_num + 4), Dzf(2 * seg_num + 4);
Dxf = Dx1.segment(0, 2 * seg_num + 4);
Dyf = Dy1.segment(0, 2 * seg_num + 4);
Dzf = Dz1.segment(0, 2 * seg_num + 4);
Eigen::MatrixXd Rff(2 * seg_num + 4, 2 * seg_num + 4);
Eigen::MatrixXd Rfp(2 * seg_num + 4, 2 * seg_num - 2);
Eigen::MatrixXd Rpf(2 * seg_num - 2, 2 * seg_num + 4);
Eigen::MatrixXd Rpp(2 * seg_num - 2, 2 * seg_num - 2);
Rff = R.block(0, 0, 2 * seg_num + 4, 2 * seg_num + 4);
Rfp = R.block(0, 2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2);
Rpf = R.block(2 * seg_num + 4, 0, 2 * seg_num - 2, 2 * seg_num + 4);
Rpp = R.block(2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2, 2 * seg_num - 2);
/* ---------- close form solution ---------- */
Eigen::VectorXd Dxp(2 * seg_num - 2), Dyp(2 * seg_num - 2), Dzp(2 * seg_num - 2);
Dxp = -(Rpp.inverse() * Rfp.transpose()) * Dxf;
Dyp = -(Rpp.inverse() * Rfp.transpose()) * Dyf;
Dzp = -(Rpp.inverse() * Rfp.transpose()) * Dzf;
Dx1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dxp;
Dy1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dyp;
Dz1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dzp;
Px = (A.inverse() * Ct) * Dx1;
Py = (A.inverse() * Ct) * Dy1;
Pz = (A.inverse() * Ct) * Dz1;
for (int i = 0; i < seg_num; i++)
{
poly_coeff.block(i, 0, 1, 6) = Px.segment(i * 6, 6).transpose();
poly_coeff.block(i, 6, 1, 6) = Py.segment(i * 6, 6).transpose();
poly_coeff.block(i, 12, 1, 6) = Pz.segment(i * 6, 6).transpose();
}
/* ---------- use polynomials ---------- */
PolynomialTraj poly_traj;
for (int i = 0; i < poly_coeff.rows(); ++i)
{
vector<double> cx(6), cy(6), cz(6);
for (int j = 0; j < 6; ++j)
{
cx[j] = poly_coeff(i, j), cy[j] = poly_coeff(i, j + 6), cz[j] = poly_coeff(i, j + 12);
}
reverse(cx.begin(), cx.end());
reverse(cy.begin(), cy.end());
reverse(cz.begin(), cz.end());
double ts = Time(i);
poly_traj.addSegment(cx, cy, cz, ts);
}
return poly_traj;
}
PolynomialTraj PolynomialTraj::one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc,
double t)
{
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(6, 6), Crow(1, 6);
Eigen::VectorXd Bx(6), By(6), Bz(6);
C(0, 5) = 1;
C(1, 4) = 1;
C(2, 3) = 2;
Crow << pow(t, 5), pow(t, 4), pow(t, 3), pow(t, 2), t, 1;
C.row(3) = Crow;
Crow << 5 * pow(t, 4), 4 * pow(t, 3), 3 * pow(t, 2), 2 * t, 1, 0;
C.row(4) = Crow;
Crow << 20 * pow(t, 3), 12 * pow(t, 2), 6 * t, 2, 0, 0;
C.row(5) = Crow;
Bx << start_pt(0), start_vel(0), start_acc(0), end_pt(0), end_vel(0), end_acc(0);
By << start_pt(1), start_vel(1), start_acc(1), end_pt(1), end_vel(1), end_acc(1);
Bz << start_pt(2), start_vel(2), start_acc(2), end_pt(2), end_vel(2), end_acc(2);
Eigen::VectorXd Cofx = C.colPivHouseholderQr().solve(Bx);
Eigen::VectorXd Cofy = C.colPivHouseholderQr().solve(By);
Eigen::VectorXd Cofz = C.colPivHouseholderQr().solve(Bz);
vector<double> cx(6), cy(6), cz(6);
for (int i = 0; i < 6; i++)
{
cx[i] = Cofx(i);
cy[i] = Cofy(i);
cz[i] = Cofz(i);
}
PolynomialTraj poly_traj;
poly_traj.addSegment(cx, cy, cz, t);
return poly_traj;
}

13
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/cmake_utils/CMakeLists.txt
Unescape View File

@ -1,13 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(cmake_utils)
find_package(catkin REQUIRED COMPONENTS roscpp)
set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake)
file(GLOB CMAKE_UILTS_FILES cmake/*.cmake)
catkin_package(
CFG_EXTRAS ${CMAKE_UILTS_FILES}
)

126
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/cmake_utils/cmake/arch.cmake
Unescape View File

@ -1,126 +0,0 @@
set(archdetect_c_code "
#if defined(__arm__) || defined(__TARGET_ARCH_ARM)
#if defined(__ARM_ARCH_7__) \\
|| defined(__ARM_ARCH_7A__) \\
|| defined(__ARM_ARCH_7R__) \\
|| defined(__ARM_ARCH_7M__) \\
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 7)
#error cmake_ARCH armv7
#elif defined(__ARM_ARCH_6__) \\
|| defined(__ARM_ARCH_6J__) \\
|| defined(__ARM_ARCH_6T2__) \\
|| defined(__ARM_ARCH_6Z__) \\
|| defined(__ARM_ARCH_6K__) \\
|| defined(__ARM_ARCH_6ZK__) \\
|| defined(__ARM_ARCH_6M__) \\
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 6)
#error cmake_ARCH armv6
#elif defined(__ARM_ARCH_5TEJ__) \\
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 5)
#error cmake_ARCH armv5
#else
#error cmake_ARCH arm
#endif
#elif defined(__i386) || defined(__i386__) || defined(_M_IX86)
#error cmake_ARCH i386
#elif defined(__x86_64) || defined(__x86_64__) || defined(__amd64) || defined(_M_X64)
#error cmake_ARCH x86_64
#elif defined(__ia64) || defined(__ia64__) || defined(_M_IA64)
#error cmake_ARCH ia64
#elif defined(__ppc__) || defined(__ppc) || defined(__powerpc__) \\
|| defined(_ARCH_COM) || defined(_ARCH_PWR) || defined(_ARCH_PPC) \\
|| defined(_M_MPPC) || defined(_M_PPC)
#if defined(__ppc64__) || defined(__powerpc64__) || defined(__64BIT__)
#error cmake_ARCH ppc64
#else
#error cmake_ARCH ppc
#endif
#endif
#error cmake_ARCH unknown
")
# Set ppc_support to TRUE before including this file or ppc and ppc64
# will be treated as invalid architectures since they are no longer supported by Apple
function(target_architecture output_var)
if(APPLE AND CMAKE_OSX_ARCHITECTURES)
# On OS X we use CMAKE_OSX_ARCHITECTURES *if* it was set
# First let's normalize the order of the values
# Note that it's not possible to compile PowerPC applications if you are using
# the OS X SDK version 10.6 or later - you'll need 10.4/10.5 for that, so we
# disable it by default
# See this page for more information:
# http://stackoverflow.com/questions/5333490/how-can-we-restore-ppc-ppc64-as-well-as-full-10-4-10-5-sdk-support-to-xcode-4
# Architecture defaults to i386 or ppc on OS X 10.5 and earlier, depending on the CPU type detected at runtime.
# On OS X 10.6+ the default is x86_64 if the CPU supports it, i386 otherwise.
foreach(osx_arch ${CMAKE_OSX_ARCHITECTURES})
if("${osx_arch}" STREQUAL "ppc" AND ppc_support)
set(osx_arch_ppc TRUE)
elseif("${osx_arch}" STREQUAL "i386")
set(osx_arch_i386 TRUE)
elseif("${osx_arch}" STREQUAL "x86_64")
set(osx_arch_x86_64 TRUE)
elseif("${osx_arch}" STREQUAL "ppc64" AND ppc_support)
set(osx_arch_ppc64 TRUE)
else()
message(FATAL_ERROR "Invalid OS X arch name: ${osx_arch}")
endif()
endforeach()
# Now add all the architectures in our normalized order
if(osx_arch_ppc)
list(APPEND ARCH ppc)
endif()
if(osx_arch_i386)
list(APPEND ARCH i386)
endif()
if(osx_arch_x86_64)
list(APPEND ARCH x86_64)
endif()
if(osx_arch_ppc64)
list(APPEND ARCH ppc64)
endif()
else()
file(WRITE "${CMAKE_BINARY_DIR}/arch.c" "${archdetect_c_code}")
enable_language(C)
# Detect the architecture in a rather creative way...
# This compiles a small C program which is a series of ifdefs that selects a
# particular #error preprocessor directive whose message string contains the
# target architecture. The program will always fail to compile (both because
# file is not a valid C program, and obviously because of the presence of the
# #error preprocessor directives... but by exploiting the preprocessor in this
# way, we can detect the correct target architecture even when cross-compiling,
# since the program itself never needs to be run (only the compiler/preprocessor)
try_run(
run_result_unused
compile_result_unused
"${CMAKE_BINARY_DIR}"
"${CMAKE_BINARY_DIR}/arch.c"
COMPILE_OUTPUT_VARIABLE ARCH
CMAKE_FLAGS CMAKE_OSX_ARCHITECTURES=${CMAKE_OSX_ARCHITECTURES}
)
# Parse the architecture name from the compiler output
string(REGEX MATCH "cmake_ARCH ([a-zA-Z0-9_]+)" ARCH "${ARCH}")
# Get rid of the value marker leaving just the architecture name
string(REPLACE "cmake_ARCH " "" ARCH "${ARCH}")
# If we are compiling with an unknown architecture this variable should
# already be set to "unknown" but in the case that it's empty (i.e. due
# to a typo in the code), then set it to unknown
if (NOT ARCH)
set(ARCH unknown)
endif()
endif()
set(${output_var} "${ARCH}" PARENT_SCOPE)
endfunction()

2
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/cmake_utils/cmake/cmake_modules.cmake
Unescape View File

@ -1,2 +0,0 @@
list(APPEND CMAKE_MODULE_PATH "${cmake_utils_SOURCE_PREFIX}/cmake_modules")
link_directories( ${cmake_utils_SOURCE_PREFIX}/lib/mosek8 )

17
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/cmake_utils/cmake/color.cmake
Unescape View File

@ -1,17 +0,0 @@
string(ASCII 27 Esc)
set(ColourReset "${Esc}[m")
set(ColourBold "${Esc}[1m")
set(Red "${Esc}[31m")
set(Green "${Esc}[32m")
set(Yellow "${Esc}[33m")
set(Blue "${Esc}[34m")
set(Magenta "${Esc}[35m")
set(Cyan "${Esc}[36m")
set(White "${Esc}[37m")
set(BoldRed "${Esc}[1;31m")
set(BoldGreen "${Esc}[1;32m")
set(BoldYellow "${Esc}[1;33m")
set(BoldBlue "${Esc}[1;34m")
set(BoldMagenta "${Esc}[1;35m")
set(BoldCyan "${Esc}[1;36m")
set(BoldWhite "${Esc}[1;37m")

173
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/cmake_utils/cmake_modules/FindEigen.cmake
Unescape View File

@ -1,173 +0,0 @@
# Ceres Solver - A fast non-linear least squares minimizer
# Copyright 2015 Google Inc. All rights reserved.
# http://ceres-solver.org/
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
# * Neither the name of Google Inc. nor the names of its contributors may be
# used to endorse or promote products derived from this software without
# specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: alexs.mac@gmail.com (Alex Stewart)
#
# FindEigen.cmake - Find Eigen library, version >= 3.
#
# This module defines the following variables:
#
# EIGEN_FOUND: TRUE iff Eigen is found.
# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
#
# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
#
# The following variables control the behaviour of this module:
#
# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
# search for eigen includes, e.g: /timbuktu/eigen3.
#
# The following variables are also defined by this module, but in line with
# CMake recommended FindPackage() module style should NOT be referenced directly
# by callers (use the plural variables detailed above instead). These variables
# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
# are NOT re-called (i.e. search for library is not repeated) if these variables
# are set with valid values _in the CMake cache_. This means that if these
# variables are set directly in the cache, either by the user in the CMake GUI,
# or by the user passing -DVAR=VALUE directives to CMake when called (which
# explicitly defines a cache variable), then they will be used verbatim,
# bypassing the HINTS variables and other hard-coded search locations.
#
# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
# include directory of any dependencies.
# Called if we failed to find Eigen or any of it's required dependencies,
# unsets all public (designed to be used externally) variables and reports
# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
unset(EIGEN_FOUND)
unset(EIGEN_INCLUDE_DIRS)
# Make results of search visible in the CMake GUI if Eigen has not
# been found so that user does not have to toggle to advanced view.
mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
# use the camelcase library name, not uppercase.
if (Eigen_FIND_QUIETLY)
message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
elseif (Eigen_FIND_REQUIRED)
message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
else()
# Neither QUIETLY nor REQUIRED, use no priority which emits a message
# but continues configuration and allows generation.
message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
endif ()
return()
endmacro(EIGEN_REPORT_NOT_FOUND)
# Protect against any alternative find_package scripts for this library having
# been called previously (in a client project) which set EIGEN_FOUND, but not
# the other variables we require / set here which could cause the search logic
# here to fail.
unset(EIGEN_FOUND)
# Search user-installed locations first, so that we prefer user installs
# to system installs where both exist.
#
# TODO: Add standard Windows search locations for Eigen.
list(APPEND EIGEN_CHECK_INCLUDE_DIRS
/usr/local/include
/usr/local/homebrew/include # Mac OS X
/opt/local/var/macports/software # Mac OS X.
/opt/local/include
/usr/include)
# Additional suffixes to try appending to each search path.
list(APPEND EIGEN_CHECK_PATH_SUFFIXES
eigen3 # Default root directory for Eigen.
Eigen/include/eigen3 ) # Windows (for C:/Program Files prefix).
# Search supplied hint directories first if supplied.
find_path(EIGEN_INCLUDE_DIR
NAMES Eigen/Core
PATHS ${EIGEN_INCLUDE_DIR_HINTS}
${EIGEN_CHECK_INCLUDE_DIRS}
PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
if (NOT EIGEN_INCLUDE_DIR OR
NOT EXISTS ${EIGEN_INCLUDE_DIR})
eigen_report_not_found(
"Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
"path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
endif (NOT EIGEN_INCLUDE_DIR OR
NOT EXISTS ${EIGEN_INCLUDE_DIR})
# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
# if called.
set(EIGEN_FOUND TRUE)
# Extract Eigen version from Eigen/src/Core/util/Macros.h
if (EIGEN_INCLUDE_DIR)
set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
if (NOT EXISTS ${EIGEN_VERSION_FILE})
eigen_report_not_found(
"Could not find file: ${EIGEN_VERSION_FILE} "
"containing version information in Eigen install located at: "
"${EIGEN_INCLUDE_DIR}.")
else (NOT EXISTS ${EIGEN_VERSION_FILE})
file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
# This is on a single line s/t CMake does not interpret it as a list of
# elements and insert ';' separators which would result in 3.;2.;0 nonsense.
set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
endif (NOT EXISTS ${EIGEN_VERSION_FILE})
endif (EIGEN_INCLUDE_DIR)
# Set standard CMake FindPackage variables if found.
if (EIGEN_FOUND)
set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
endif (EIGEN_FOUND)
# Handle REQUIRED / QUIET optional arguments and version.
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen
REQUIRED_VARS EIGEN_INCLUDE_DIRS
VERSION_VAR EIGEN_VERSION)
# Only mark internal variables as advanced if we found Eigen, otherwise
# leave it visible in the standard GUI for the user to set manually.
if (EIGEN_FOUND)
mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)
endif (EIGEN_FOUND)

135
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/cmake_utils/cmake_modules/FindGSL.cmake
Unescape View File

@ -1,135 +0,0 @@
# Try to find gnu scientific library GSL
# See
# http://www.gnu.org/software/gsl/ and
# http://gnuwin32.sourceforge.net/packages/gsl.htm
#
# Based on a script of Felix Woelk and Jan Woetzel
# (www.mip.informatik.uni-kiel.de)
#
# It defines the following variables:
# GSL_FOUND - system has GSL lib
# GSL_INCLUDE_DIRS - where to find headers
# GSL_LIBRARIES - full path to the libraries
# GSL_LIBRARY_DIRS, the directory where the PLplot library is found.
# CMAKE_GSL_CXX_FLAGS = Unix compiler flags for GSL, essentially "`gsl-config --cxxflags`"
# GSL_LINK_DIRECTORIES = link directories, useful for rpath on Unix
# GSL_EXE_LINKER_FLAGS = rpath on Unix
set( GSL_FOUND OFF )
set( GSL_CBLAS_FOUND OFF )
# Windows, but not for Cygwin and MSys where gsl-config is available
if( WIN32 AND NOT CYGWIN AND NOT MSYS )
# look for headers
find_path( GSL_INCLUDE_DIR
NAMES gsl/gsl_cdf.h gsl/gsl_randist.h
)
if( GSL_INCLUDE_DIR )
# look for gsl library
find_library( GSL_LIBRARY
NAMES gsl
)
if( GSL_LIBRARY )
set( GSL_INCLUDE_DIRS ${GSL_INCLUDE_DIR} )
get_filename_component( GSL_LIBRARY_DIRS ${GSL_LIBRARY} PATH )
set( GSL_FOUND ON )
endif( GSL_LIBRARY )
# look for gsl cblas library
find_library( GSL_CBLAS_LIBRARY
NAMES gslcblas
)
if( GSL_CBLAS_LIBRARY )
set( GSL_CBLAS_FOUND ON )
endif( GSL_CBLAS_LIBRARY )
set( GSL_LIBRARIES ${GSL_LIBRARY} ${GSL_CBLAS_LIBRARY} )
endif( GSL_INCLUDE_DIR )
mark_as_advanced(
GSL_INCLUDE_DIR
GSL_LIBRARY
GSL_CBLAS_LIBRARY
)
else( WIN32 AND NOT CYGWIN AND NOT MSYS )
if( UNIX OR MSYS )
find_program( GSL_CONFIG_EXECUTABLE gsl-config
/usr/bin/
/usr/local/bin
)
if( GSL_CONFIG_EXECUTABLE )
set( GSL_FOUND ON )
# run the gsl-config program to get cxxflags
execute_process(
COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --cflags
OUTPUT_VARIABLE GSL_CFLAGS
RESULT_VARIABLE RET
ERROR_QUIET
)
if( RET EQUAL 0 )
string( STRIP "${GSL_CFLAGS}" GSL_CFLAGS )
separate_arguments( GSL_CFLAGS )
# parse definitions from cflags; drop -D* from CFLAGS
string( REGEX MATCHALL "-D[^;]+"
GSL_DEFINITIONS "${GSL_CFLAGS}" )
string( REGEX REPLACE "-D[^;]+;" ""
GSL_CFLAGS "${GSL_CFLAGS}" )
# parse include dirs from cflags; drop -I prefix
string( REGEX MATCHALL "-I[^;]+"
GSL_INCLUDE_DIRS "${GSL_CFLAGS}" )
string( REPLACE "-I" ""
GSL_INCLUDE_DIRS "${GSL_INCLUDE_DIRS}")
string( REGEX REPLACE "-I[^;]+;" ""
GSL_CFLAGS "${GSL_CFLAGS}")
message("GSL_DEFINITIONS=${GSL_DEFINITIONS}")
message("GSL_INCLUDE_DIRS=${GSL_INCLUDE_DIRS}")
message("GSL_CFLAGS=${GSL_CFLAGS}")
else( RET EQUAL 0 )
set( GSL_FOUND FALSE )
endif( RET EQUAL 0 )
# run the gsl-config program to get the libs
execute_process(
COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --libs
OUTPUT_VARIABLE GSL_LIBRARIES
RESULT_VARIABLE RET
ERROR_QUIET
)
if( RET EQUAL 0 )
string(STRIP "${GSL_LIBRARIES}" GSL_LIBRARIES )
separate_arguments( GSL_LIBRARIES )
# extract linkdirs (-L) for rpath (i.e., LINK_DIRECTORIES)
string( REGEX MATCHALL "-L[^;]+"
GSL_LIBRARY_DIRS "${GSL_LIBRARIES}" )
string( REPLACE "-L" ""
GSL_LIBRARY_DIRS "${GSL_LIBRARY_DIRS}" )
else( RET EQUAL 0 )
set( GSL_FOUND FALSE )
endif( RET EQUAL 0 )
MARK_AS_ADVANCED(
GSL_CFLAGS
)
message( STATUS "Using GSL from ${GSL_PREFIX}" )
else( GSL_CONFIG_EXECUTABLE )
message( STATUS "FindGSL: gsl-config not found.")
endif( GSL_CONFIG_EXECUTABLE )
endif( UNIX OR MSYS )
endif( WIN32 AND NOT CYGWIN AND NOT MSYS )
if( GSL_FOUND )
if( NOT GSL_FIND_QUIETLY )
message( STATUS "FindGSL: Found both GSL headers and library" )
endif( NOT GSL_FIND_QUIETLY )
else( GSL_FOUND )
if( GSL_FIND_REQUIRED )
message( FATAL_ERROR "FindGSL: Could not find GSL headers or library" )
endif( GSL_FIND_REQUIRED )
endif( GSL_FOUND )

124
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake
Unescape View File

@ -1,124 +0,0 @@
macro(mvIMPACT_REPORT_NOT_FOUND REASON_MSG)
unset(mvIMPACT_FOUND)
unset(mvIMPACT_INCLUDE_DIRS)
unset(mvIMPACT_LIBRARIES)
unset(mvIMPACT_WORLD_VERSION)
unset(mvIMPACT_MAJOR_VERSION)
unset(mvIMPACT_MINOR_VERSION)
# Make results of search visible in the CMake GUI if mvimpact has not
# been found so that user does not have to toggle to advanced view.
mark_as_advanced(CLEAR mvIMPACT_INCLUDE_DIR)
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
# use the camelcase library name, not uppercase.
if(Mvimpact_FIND_QUIETLY)
message(STATUS "Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
elseif(Mvimpact_FIND_REQUIRED)
message(FATAL_ERROR "Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
else()
# Neither QUIETLY nor REQUIRED, use no priority which emits a message
# but continues configuration and allows generation.
message("-- Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
endif()
endmacro(mvIMPACT_REPORT_NOT_FOUND)
# Search user-installed locations first, so that we prefer user installs
# to system installs where both exist.
get_filename_component(BLUEFOX2_DIR ${CMAKE_CURRENT_SOURCE_DIR} REALPATH)
list(APPEND mvIMPACT_CHECK_INCLUDE_DIRS
/opt/mvIMPACT_acquire
/opt/mvIMPACT_Acquire
)
execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCH)
if(NOT ARCH)
set(ARCH "arm64")
elseif(${ARCH} STREQUAL "aarch64")
set(ARCH "arm64")
endif()
list(APPEND mvIMPACT_CHECK_LIBRARY_DIRS
/opt/mvIMPACT_acquire/lib/${ARCH}
/opt/mvIMPACT_Acquire/lib/${ARCH}
)
# Check general hints
if(mvIMPACT_HINTS AND EXISTS ${mvIMPACT_HINTS})
set(mvIMPACT_INCLUDE_DIR_HINTS ${mvIMPACT_HINTS}/include)
set(mvIMPACT_LIBRARY_DIR_HINTS ${mvIMPACT_HINTS}/lib)
endif()
# Search supplied hint directories first if supplied.
# Find include directory for mvimpact
find_path(mvIMPACT_INCLUDE_DIR
NAMES mvIMPACT_CPP/mvIMPACT_acquire.h
PATHS ${mvIMPACT_INCLUDE_DIR_HINTS}
${mvIMPACT_CHECK_INCLUDE_DIRS}
NO_DEFAULT_PATH)
if(NOT mvIMPACT_INCLUDE_DIR OR NOT EXISTS ${mvIMPACT_INCLUDE_DIR})
mvIMPACT_REPORT_NOT_FOUND(
"Could not find mvimpact include directory, set mvIMPACT_INCLUDE_DIR to "
"path to mvimpact include directory,"
"e.g. /opt/mvIMPACT_acquire.")
else()
message(STATUS "mvimpact include dir found: " ${mvIMPACT_INCLUDE_DIR})
endif()
# Find library directory for mvimpact
find_library(mvIMPACT_LIBRARY
NAMES libmvBlueFOX.so
PATHS ${mvIMPACT_LIBRARY_DIR_HINTS}
${mvIMPACT_CHECK_LIBRARY_DIRS}
NO_DEFAULT_PATH)
if(NOT mvIMPACT_LIBRARY OR NOT EXISTS ${mvIMPACT_LIBRARY})
mvIMPACT_REPORT_NOT_FOUND(
"Could not find mvimpact library, set mvIMPACT_LIBRARY "
"to full path to mvimpact library direcotory.${mvIMPACT_CHECK_LIBRARY_DIRS}~~")
else()
# TODO: need to fix this hacky solution for getting mvIMPACT_LIBRARY_DIR
string(REGEX MATCH ".*/" mvIMPACT_LIBRARY_DIR ${mvIMPACT_LIBRARY})
message(STATUS "mvimpact library dir found: " ${mvIMPACT_LIBRARY_DIR})
endif()
# Mark internally as found, then verify. mvIMPACT_REPORT_NOT_FOUND() unsets if
# called.
set(mvIMPACT_FOUND TRUE)
# Extract mvimpact version
if(mvIMPACT_LIBRARY_DIR)
file(GLOB mvIMPACT_LIBS
RELATIVE ${mvIMPACT_LIBRARY_DIR}
${mvIMPACT_LIBRARY_DIR}/libmvBlueFOX.so.[0-9].[0-9].[0-9])
# TODO: add version support
# string(REGEX MATCH ""
# mvIMPACT_WORLD_VERSION ${mvIMPACT_PVBASE})
# message(STATUS "mvimpact world version: " ${mvIMPACT_WORLD_VERSION})
endif()
# Catch case when caller has set mvIMPACT_INCLUDE_DIR in the cache / GUI and
# thus FIND_[PATH/LIBRARY] are not called, but specified locations are
# invalid, otherwise we would report the library as found.
if(NOT mvIMPACT_INCLUDE_DIR AND NOT EXISTS ${mvIMPACT_INCLUDE_DIR}/mvIMPACT_CPP/mvIMPACT_acquire.h)
mvIMPACT_REPORT_NOT_FOUND("Caller defined mvIMPACT_INCLUDE_DIR: "
${mvIMPACT_INCLUDE_DIR}
" does not contain mvIMPACT_CPP/mvIMPACT_acquire.h header.")
endif()
# Set standard CMake FindPackage variables if found.
if(mvIMPACT_FOUND)
set(mvIMPACT_INCLUDE_DIRS ${mvIMPACT_INCLUDE_DIR})
file(GLOB mvIMPACT_LIBRARIES ${mvIMPACT_LIBRARY_DIR}lib*.so)
endif()
# Handle REQUIRED / QUIET optional arguments.
include(FindPackageHandleStandardArgs)
if(mvIMPACT_FOUND)
FIND_PACKAGE_HANDLE_STANDARD_ARGS(Mvimpact DEFAULT_MSG
mvIMPACT_INCLUDE_DIRS mvIMPACT_LIBRARIES)
endif()
# Only mark internal variables as advanced if we found mvimpact, otherwise
# leave it visible in the standard GUI for the user to set manually.
if(mvIMPACT_FOUND)
mark_as_advanced(FORCE mvIMPACT_INCLUDE_DIR mvIMPACT_LIBRARY)
endif()

18
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/cmake_utils/package.xml
Unescape View File

@ -1,18 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>cmake_utils</name>
<version>0.0.0</version>
<description>
Once you used this package,
then you do not need to copy cmake files among packages
</description>
<maintainer email="william.wu@dji.com"> William.Wu </maintainer>
<license>LGPLv3</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>cmake_modules</depend>
</package>

BIN
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server.zip
View File

Binary file not shown.

222
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/CMakeLists.txt
Unescape View File

@ -1,222 +0,0 @@
#cmake_minimum_required(VERSION 2.4.6)
#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE Release)
#rosbuild_init()
#set the default path for built executables to the "bin" directory
#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
#rosbuild_add_boost_directories()
#rosbuild_add_executable(multi_map_server src/multi_map_server.cc)
#target_link_libraries(multi_map_server pose_utils)
#rosbuild_add_executable(multi_map_visualization src/multi_map_visualization.cc)
#target_link_libraries(multi_map_visualization pose_utils)
#----------------------------------
cmake_minimum_required(VERSION 2.8.3)
project(multi_map_server)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
pose_utils
message_generation
roscpp
visualization_msgs
tf
std_srvs
laser_geometry
pose_utils
quadrotor_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependencies might have been
## pulled in transitively but can be declared for certainty nonetheless:
## * add a build_depend tag for "message_generation"
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
MultiOccupancyGrid.msg
MultiSparseMap3D.msg
SparseMap3D.msg
VerticalOccupancyGridList.msg
#plan_cmd.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
geometry_msgs
nav_msgs
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES irobot_msgs
CATKIN_DEPENDS geometry_msgs nav_msgs quadrotor_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
find_package(Armadillo REQUIRED)
include_directories(${ARMADILLO_INCLUDE_DIRS})
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
## Declare a cpp library
# add_library(irobot_msgs
# src/${PROJECT_NAME}/irobot_msgs.cpp
# )
## Declare a cpp executable
add_executable(multi_map_visualization src/multi_map_visualization.cc)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
add_dependencies(multi_map_visualization multi_map_server_messages_cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(multi_map_visualization
${catkin_LIBRARIES}
${ARMADILLO_LIBRARIES}
${pose_utils_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS irobot_msgs irobot_msgs_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_irobot_msgs.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

1
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/Makefile
Unescape View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

242
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map2D.h
Unescape View File

@ -1,242 +0,0 @@
#ifndef MAP2D_H
#define MAP2D_H
#include <iostream>
#include <ros/ros.h>
#include <tf/tf.h>
#include <nav_msgs/OccupancyGrid.h>
using namespace std;
class Map2D
{
private:
nav_msgs::OccupancyGrid map;
int expandStep;
int binning;
bool isBinningSet;
bool updated;
public:
Map2D()
{
map.data.resize(0);
map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);
expandStep = 200;
binning = 1;
isBinningSet = false;
updated = false;
}
Map2D(int _binning)
{
map.data.resize(0);
map.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);
expandStep = 200;
binning = _binning;
isBinningSet = true;
updated = false;
}
~Map2D() {}
double GetResolution() { return map.info.resolution; }
double GetMinX() { return map.info.origin.position.x; }
double GetMinY() { return map.info.origin.position.y; }
double GetMaxX() { return map.info.origin.position.x + map.info.width * map.info.resolution; }
double GetMaxY() { return map.info.origin.position.y + map.info.height * map.info.resolution; }
bool Updated() { return updated; }
void Reset() { map = nav_msgs::OccupancyGrid(); }
void SetBinning(int _binning)
{
if (!isBinningSet)
binning = _binning;
}
// Get occupancy value, 0: unknown; +ve: occupied; -ve: free
signed char GetOccupiedFromWorldFrame(double x, double y)
{
int xm = (x - map.info.origin.position.x) / map.info.resolution;
int ym = (y - map.info.origin.position.y) / map.info.resolution;
if (xm < 0 || xm > (int)(map.info.width-1) || ym < 0 || ym > (int)(map.info.height-1))
return 0;
else
return map.data[ym*map.info.width+xm];
}
void Replace(nav_msgs::OccupancyGrid m)
{
// Check data
if (m.data.size() == 0)
return;
isBinningSet = true;
// Binning, conservative, take maximum
if (binning > 1)
{
int _width = m.info.width;
m.info.width /= binning;
m.info.height /= binning;
m.info.resolution *= binning;
vector<signed char> data(m.info.width * m.info.height);
for (uint32_t i = 0; i < m.info.height; i++)
{
for (uint32_t j = 0; j < m.info.width; j++)
{
int val = -0xff;
for (int _i = 0; _i < binning; _i++)
{
for (int _j = 0; _j < binning; _j++)
{
int v = m.data[(i*binning + _i) * _width + (j*binning + _j)];
val = (v > val)?v:val;
}
}
data[i * m.info.width + j] = val;
}
}
m.data = data;
}
// Replace map
map = m;
updated = true;
}
// Merge submap
void Update(nav_msgs::OccupancyGrid m)
{
// Check data
if (m.data.size() == 0)
return;
isBinningSet = true;
// Binning, conservative, take maximum
if (binning > 1)
{
int _width = m.info.width;
m.info.width /= binning;
m.info.height /= binning;
m.info.resolution *= binning;
vector<signed char> data(m.info.width * m.info.height);
for (uint32_t i = 0; i < m.info.height; i++)
{
for (uint32_t j = 0; j < m.info.width; j++)
{
int val = -0xff;
for (int _i = 0; _i < binning; _i++)
{
for (int _j = 0; _j < binning; _j++)
{
int v = m.data[(i*binning + _i) * _width + (j*binning + _j)];
val = (v > val)?v:val;
}
}
data[i * m.info.width + j] = val;
}
}
m.data = data;
}
// Initialize and check resolution
if (map.info.resolution == 0)
map.info.resolution = m.info.resolution;
else if (m.info.resolution != map.info.resolution)
return;
// Get Info
double ox = m.info.origin.position.x;
double oy = m.info.origin.position.y;
double oyaw = tf::getYaw(m.info.origin.orientation);
double syaw = sin(oyaw);
double cyaw = cos(oyaw);
int mx = m.info.width;
int my = m.info.height;
double xs[4];
double ys[4];
xs[0] = cyaw * (0) - syaw * (0) + ox;
xs[1] = cyaw * (mx*map.info.resolution) - syaw * (0) + ox;
xs[2] = cyaw * (0) - syaw * (mx*map.info.resolution) + ox;
xs[3] = cyaw * (mx*map.info.resolution) - syaw * (my*map.info.resolution) + ox;
ys[0] = syaw * (0) + cyaw * (0) + oy;
ys[1] = syaw * (mx*map.info.resolution) + cyaw * (0) + oy;
ys[2] = syaw * (0) + cyaw * (my*map.info.resolution) + oy;
ys[3] = syaw * (mx*map.info.resolution) + cyaw * (my*map.info.resolution) + oy;
double minx = xs[0];
double maxx = xs[0];
double miny = ys[0];
double maxy = ys[0];
for (int k = 0; k < 4; k++)
{
minx = (xs[k] < minx)?xs[k]:minx;
miny = (ys[k] < miny)?ys[k]:miny;
maxx = (xs[k] > maxx)?xs[k]:maxx;
maxy = (ys[k] > maxy)?ys[k]:maxy;
}
// Check whether resize map is needed
bool mapResetFlag = false;
double prevOriginX = map.info.origin.position.x;
double prevOriginY = map.info.origin.position.y;
int prevMapX = map.info.width;
int prevMapY = map.info.height;
while (map.info.origin.position.x > minx)
{
map.info.width += expandStep;
map.info.origin.position.x -= expandStep*map.info.resolution;
mapResetFlag = true;
}
while (map.info.origin.position.y > miny)
{
map.info.height += expandStep;
map.info.origin.position.y -= expandStep*map.info.resolution;
mapResetFlag = true;
}
while (map.info.origin.position.x + map.info.width*map.info.resolution < maxx)
{
map.info.width += expandStep;
mapResetFlag = true;
}
while (map.info.origin.position.y + map.info.height*map.info.resolution < maxy)
{
map.info.height += expandStep;
mapResetFlag = true;
}
// Resize map
if (mapResetFlag)
{
mapResetFlag = false;
vector<signed char> _data = map.data;
map.data.clear();
map.data.resize(map.info.width*map.info.height,0);
for (int x = 0; x < prevMapX; x++)
{
for(int y = 0; y < prevMapY; y++)
{
int xn = x + round((prevOriginX - map.info.origin.position.x) / map.info.resolution);
int yn = y + round((prevOriginY - map.info.origin.position.y) / map.info.resolution);
map.data[yn*map.info.width+xn] = _data[y*prevMapX+x];
}
}
}
// Merge map
for (int x = 0; x < mx; x++)
{
for(int y = 0; y < my; y++)
{
int xn = (cyaw*(x*map.info.resolution)-syaw*(y*map.info.resolution)+ox-map.info.origin.position.x) / map.info.resolution;
int yn = (syaw*(x*map.info.resolution)+cyaw*(y*map.info.resolution)+oy-map.info.origin.position.y) / map.info.resolution;
if (abs((int)(map.data[yn*map.info.width+xn]) + (int)(m.data[y*mx+x])) <= 127)
map.data[yn*map.info.width+xn] += m.data[y*mx+x];
}
}
updated = true;
}
const nav_msgs::OccupancyGrid& GetMap()
{
map.header.stamp = ros::Time::now();
map.info.map_load_time = ros::Time::now();
map.header.frame_id = string("/map");
updated = false;
return map;
}
};
#endif

608
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map3D.h
Unescape View File

@ -1,608 +0,0 @@
#ifndef MAP3D_H
#define MAP3D_H
#include <iostream>
#include <ros/ros.h>
#include <tf/tf.h>
#include <armadillo>
#include <multi_map_server/SparseMap3D.h>
using namespace std;
// Occupancy probability of a sensor
#define PROB_OCCUPIED 0.95
// Free space probability of a sensor
#define PROB_FREE 0.01
// Threshold that determine a cell is occupied
#define PROB_OCCUPIED_THRESHOLD 0.75
// If beyond this threshold, the occupancy(occupied) of this cell is fixed, no decay
#define PROB_OCCUPIED_FIXED_THRESHOLD 0.95
// Threshold that determine a cell is free
#define PROB_FREE_THRESHOLD 0.25
// If bwlow this threshold, the occupancy(free) of this cell is fixed, no decay
#define PROB_FREE_FIXED_THRESHOLD 0.05
// Used integer value to store occupancy, create a large scale factor to enable small scale decay
#define LOG_ODD_SCALE_FACTOR 10000
// Decay factor per second
#define LOG_ODD_DECAY_RATE 1.00
// Binary value of the occupancy output
#define OCCUPIED 1
#define FREE -1
// Cell Struct -----------------------------------------
struct OccupancyGrid
{
int upper;
int lower;
int mass;
};
// Occupancy Grids List --------------------------------
class OccupancyGridList
{
public:
OccupancyGridList() { updateCounter = 0; }
~OccupancyGridList() { }
void PackMsg(multi_map_server::VerticalOccupancyGridList &msg)
{
msg.x = x;
msg.y = y;
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
{
msg.upper.push_back(k->upper);
msg.lower.push_back(k->lower);
msg.mass.push_back(k->mass);
}
}
void UnpackMsg(const multi_map_server::VerticalOccupancyGridList &msg)
{
x = msg.x;
y = msg.y;
updateCounter = 0;
grids.clear();
for (unsigned int k = 0; k < msg.mass.size(); k++)
{
OccupancyGrid c;
c.upper = msg.upper[k];
c.lower = msg.lower[k];
c.mass = msg.mass[k];
grids.push_back(c);
}
}
void GetOccupancyGrids(vector<OccupancyGrid>& _grids)
{
_grids.clear();
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
_grids.push_back((*k));
}
inline int GetUpdateCounter() { return updateCounter; }
inline void SetUpdateCounterXY(int _updateCounter, double _x, double _y)
{
updateCounter = _updateCounter;
x = _x;
y = _y;
}
inline int GetOccupancyValue(int mz)
{
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
if (mz <= k->upper && mz >= k->lower)
return k->mass / (k->upper - k->lower + 1);
return 0;
}
inline void DeleteOccupancyGrid(int mz)
{
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
{
if (mz <= k->upper && mz >= k->lower)
{
grids.erase(k);
return;
}
}
return;
}
inline void SetOccupancyValue(int mz, int value)
{
OccupancyGrid grid;
grid.upper = mz;
grid.lower = mz;
grid.mass = value;
list<OccupancyGrid>::iterator gend = grids.end();
gend--;
if (grids.size() == 0) // Empty case
{
grids.push_back(grid);
return;
}
else if (mz - grids.begin()->upper > 1) // Beyond highest
{
grids.push_front(grid);
return;
}
else if (mz - grids.begin()->upper == 1) // Next to highest
{
grids.begin()->upper += 1;
grids.begin()->mass += value;
return;
}
else if (gend->lower - mz > 1) // Below lowest
{
grids.push_back(grid);
return;
}
else if (gend->lower - mz == 1) // Next to lowest
{
grids.end()->lower -= 1;
grids.end()->mass += value;
return;
}
else // General case
{
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
{
if (mz <= k->upper && mz >= k->lower) // Within a grid
{
k->mass += value;
return;
}
else if (k != gend)
{
list<OccupancyGrid>::iterator j = k;
j++;
if (k->lower - mz == 1 && mz - j->upper > 1) // ###*--###
{
k->lower -= 1;
k->mass += value;
return;
}
else if (k->lower - mz > 1 && mz - j->upper == 1) // ###--*###
{
j->upper += 1;
j->mass += value;
return;
}
else if (k->lower - mz == 1 && mz - j->upper == 1) // ###*###
{
k->lower = j->lower;
k->mass += j->mass + value;
grids.erase(j);
return;
}
else if (k->lower - mz > 1 && mz - j->upper > 1) // ###-*-###
{
grids.insert(j, grid);
return;
}
}
}
}
}
// Merging two columns, merge the grids in input "gridList" into current column
inline void Merge(const OccupancyGridList& gridsIn)
{
// Create a sorted list containing both upper and lower values
list<pair<int, int> > lp;
for (list<OccupancyGrid>::const_iterator k = grids.begin(); k != grids.end(); k++)
{
lp.push_back( make_pair(k->upper, k->mass));
lp.push_back( make_pair(k->lower, -1));
}
list<pair<int, int> > lp2;
for (list<OccupancyGrid>::const_iterator k = gridsIn.grids.begin(); k != gridsIn.grids.end(); k++)
{
lp2.push_back( make_pair(k->upper, k->mass));
lp2.push_back( make_pair(k->lower, -1));
}
lp.merge(lp2, ComparePair());
// Manipulate this list to get a minimum size list
grids.clear();
int currUpper = 0;
int currLower = 0;
int currMass = 0;
int upperCnt = 0;
int lowerCnt = 0;
list<pair<int, int> >::iterator lend = lp.end();
lend--;
for (list<pair<int, int> >::iterator k = lp.begin(); k != lp.end(); k++)
{
if (k->second > 0)
{
if (upperCnt == 0) currUpper = k->first;
currMass = (k->second > currMass)?k->second:currMass;
upperCnt++;
}
if (k->second < 0)
{
currLower = k->first;
lowerCnt++;
}
if (lowerCnt == upperCnt && k != lend)
{
list<pair<int, int> >::iterator j = k;
if (k->first - (++j)->first == 1) continue;
}
if (lowerCnt == upperCnt)
{
OccupancyGrid c;
c.upper = currUpper;
c.lower = currLower;
c.mass = currMass;
grids.push_back(c);
upperCnt = lowerCnt = currUpper = currLower = currMass = 0;
}
}
}
inline void Decay(int upThr, int lowThr, double factor)
{
for (list<OccupancyGrid>::iterator k = grids.begin(); k != grids.end(); k++)
{
int val = k->mass / (k->upper - k->lower + 1);
if (val < upThr && val > lowThr)
k->mass *= factor;
}
}
private:
struct ComparePair
{
bool operator()(pair<int, int> p1, pair<int, int> p2)
{
if (p1.first != p2.first)
return (p1.first > p2.first);
else
return (p1.second > p2.second);
}
};
// List of vertical occupancy values
list<OccupancyGrid> grids;
// Location of the list in world frame
double x;
double y;
// Update indicator
int updateCounter;
};
// 3D Map Object ---------------------------------
class Map3D
{
public:
Map3D()
{
resolution = 0.1;
decayInterval = -1;
originX = -5;
originY = -5;
originZ = 0;
mapX = 200;
mapY = 200;
expandStep = 200;
updated = false;
updateCounter = 1;
updateList.clear();
mapBase.clear();
mapBase.resize(mapX*mapY, NULL);
logOddOccupied = log(PROB_OCCUPIED/(1.0-PROB_OCCUPIED)) * LOG_ODD_SCALE_FACTOR;
logOddFree = log(PROB_FREE/(1.0-PROB_FREE)) * LOG_ODD_SCALE_FACTOR;
logOddOccupiedThr = log(1.0/(1.0-PROB_OCCUPIED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
logOddOccupiedFixedThr = log(1.0/(1.0-PROB_OCCUPIED_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
logOddFreeThr = log(1.0/(1.0-PROB_FREE_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
logOddFreeFixedThr = log(1.0/(1.0-PROB_FREE_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
}
Map3D(const Map3D& _map3d)
{
resolution = _map3d.resolution;
decayInterval = _map3d.decayInterval;
originX = _map3d.originX;
originY = _map3d.originY;
originZ = _map3d.originZ;
mapX = _map3d.mapX;
mapY = _map3d.mapY;
expandStep = _map3d.expandStep;
updated = _map3d.updated;
updateCounter = _map3d.updateCounter;
updateList = _map3d.updateList;
mapBase = _map3d.mapBase;
for (unsigned int k = 0; k < mapBase.size(); k++)
{
if (mapBase[k])
{
mapBase[k] = new OccupancyGridList;
*mapBase[k] = *_map3d.mapBase[k];
}
}
logOddOccupied = log(PROB_OCCUPIED/(1.0-PROB_OCCUPIED)) * LOG_ODD_SCALE_FACTOR;
logOddFree = log(PROB_FREE/(1.0-PROB_FREE)) * LOG_ODD_SCALE_FACTOR;
logOddOccupiedThr = log(1.0/(1.0-PROB_OCCUPIED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
logOddOccupiedFixedThr = log(1.0/(1.0-PROB_OCCUPIED_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
logOddFreeThr = log(1.0/(1.0-PROB_FREE_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
logOddFreeFixedThr = log(1.0/(1.0-PROB_FREE_FIXED_THRESHOLD) - 1.0) * LOG_ODD_SCALE_FACTOR;
}
~Map3D()
{
for (unsigned int k = 0; k < mapBase.size(); k++)
{
if (mapBase[k])
{
delete mapBase[k];
mapBase[k] = NULL;
}
}
}
void PackMsg(multi_map_server::SparseMap3D &msg)
{
// Basic map info
msg.header.stamp = ros::Time::now();
msg.header.frame_id = string("/map");
msg.info.map_load_time = ros::Time::now();
msg.info.resolution = resolution;
msg.info.origin.position.x = originX;
msg.info.origin.position.y = originY;
msg.info.origin.position.z = originZ;
msg.info.width = mapX;
msg.info.height = mapY;
msg.info.origin.orientation = tf::createQuaternionMsgFromYaw(0.0);
// Pack columns into message
msg.lists.clear();
for (unsigned int k = 0; k < updateList.size(); k++)
{
multi_map_server::VerticalOccupancyGridList c;
updateList[k]->PackMsg(c);
msg.lists.push_back(c);
}
updateList.clear();
updateCounter++;
}
void UnpackMsg(const multi_map_server::SparseMap3D &msg)
{
// Unpack column msgs, Replace the whole column
for (unsigned int k = 0; k < msg.lists.size(); k++)
{
int mx, my, mz;
WorldFrameToMapFrame(msg.lists[k].x, msg.lists[k].y, 0, mx, my, mz);
ResizeMapBase(mx, my);
if (mapBase[my*mapX+mx]) delete mapBase[my*mapX+mx];
mapBase[my*mapX+mx] = new OccupancyGridList;
mapBase[my*mapX+mx]->UnpackMsg(msg.lists[k]);
}
CheckDecayMap();
updated = true;
}
inline void SetOccupancyFromWorldFrame(double x, double y, double z, double p = PROB_OCCUPIED)
{
// Find occupancy value to be set
int value = 0;
if (p == PROB_OCCUPIED) value = logOddOccupied;
else if (p == PROB_FREE) value = logOddFree;
else return;
// Update occupancy value, return the column that have been changed
int mx, my, mz;
WorldFrameToMapFrame(x, y, z, mx, my, mz);
ResizeMapBase(mx, my);
if (!mapBase[my*mapX+mx])
mapBase[my*mapX+mx] = new OccupancyGridList;
mapBase[my*mapX+mx]->SetOccupancyValue(mz, value);
// Also record the column that have been changed in another list, for publish incremental map
if (mapBase[my*mapX+mx]->GetUpdateCounter() != updateCounter)
{
updateList.push_back(mapBase[my*mapX+mx]);
mapBase[my*mapX+mx]->SetUpdateCounterXY(updateCounter, x, y);
}
updated = true;
}
inline int GetOccupancyFromWorldFrame(double x, double y, double z)
{
int mx, my, mz;
WorldFrameToMapFrame(x, y, z, mx, my, mz);
if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
return 0;
if (!mapBase[my*mapX+mx])
return 0;
else
{
if (mapBase[my*mapX+mx]->GetOccupancyValue(mz) > logOddOccupiedThr)
return 1;
else if (mapBase[my*mapX+mx]->GetOccupancyValue(mz) < logOddFreeThr)
return -1;
else
return 0;
}
}
inline void DeleteFromWorldFrame(double x, double y, double z)
{
int mx, my, mz;
WorldFrameToMapFrame(x, y, z, mx, my, mz);
if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
return;
if (mapBase[my*mapX+mx])
mapBase[my*mapX+mx]->DeleteOccupancyGrid(mz);
}
vector<arma::colvec>& GetOccupancyWorldFrame(int type = OCCUPIED)
{
pts.clear();
for (int mx = 0; mx < mapX; mx++)
{
for (int my = 0; my < mapY; my++)
{
if (mapBase[my*mapX+mx])
{
vector<OccupancyGrid> grids;
mapBase[my*mapX+mx]->GetOccupancyGrids(grids);
for (unsigned int k = 0; k < grids.size(); k++)
{
if ( (grids[k].mass / (grids[k].upper - grids[k].lower + 1) > logOddOccupiedThr && type == OCCUPIED) ||
(grids[k].mass / (grids[k].upper - grids[k].lower + 1) < logOddFreeThr && type == FREE) )
{
for (int mz = grids[k].lower; mz <= grids[k].upper; mz++)
{
double x, y, z;
MapFrameToWorldFrame(mx, my, mz, x, y, z);
arma::colvec pt(3);
pt(0) = x;
pt(1) = y;
pt(2) = z;
pts.push_back(pt);
}
}
}
}
}
}
return pts;
}
// Do not allow setting parameters if at least one update received
void SetResolution(double _resolution)
{
if (!updated)
resolution = _resolution;
}
void SetDecayInterval(double _decayInterval)
{
if (!updated && _decayInterval > 0)
decayInterval = _decayInterval;
}
inline double GetResolution() { return resolution; }
inline double GetMaxX() { return originX + mapX*resolution; }
inline double GetMinX() { return originX; }
inline double GetMaxY() { return originY + mapY*resolution; }
inline double GetMinY() { return originY; }
inline bool Updated() { return updated; }
private:
inline void WorldFrameToMapFrame(double x, double y, double z, int& mx, int& my, int& mz)
{
mx = (x - originX) / resolution;
my = (y - originY) / resolution;
mz = (z - originZ) / resolution;
}
inline void MapFrameToWorldFrame(int mx, int my, int mz, double& x, double& y, double& z)
{
double r = 0.5*resolution;
x = mx * resolution + r + originX;
y = my * resolution + r + originY;
z = mz * resolution + r + originZ;
}
inline void ResizeMapBase(int& mx, int& my)
{
if (mx < 0 || my < 0 || mx >= mapX || my >= mapY)
{
double prevOriginX = originX;
double prevOriginY = originY;
int prevMapX = mapX;
int prevMapY = mapY;
while(mx < 0)
{
mapX += expandStep;
mx += expandStep;
originX -= expandStep*resolution;
}
while(my < 0)
{
mapY += expandStep;
my += expandStep;
originY -= expandStep*resolution;
}
while(mx >= mapX)
{
mapX += expandStep;
}
while(my >= mapY)
{
mapY += expandStep;
}
vector<OccupancyGridList*> _mapBase = mapBase;
mapBase.clear();
mapBase.resize(mapX*mapY,NULL);
for (int _x = 0; _x < prevMapX; _x++)
{
for(int _y = 0; _y < prevMapY; _y++)
{
int x = _x + round((prevOriginX - originX) / resolution);
int y = _y + round((prevOriginY - originY) / resolution);
mapBase[y*mapX+x] = _mapBase[_y*prevMapX+_x];
}
}
}
}
void CheckDecayMap()
{
if (decayInterval < 0)
return;
// Check whether to decay
static ros::Time prevDecayT = ros::Time::now();
ros::Time t = ros::Time::now();
double dt = (t - prevDecayT).toSec();
if (dt > decayInterval)
{
double r = pow(LOG_ODD_DECAY_RATE, dt);
for (int mx = 0; mx < mapX; mx++)
for (int my = 0; my < mapY; my++)
if (mapBase[my*mapX+mx])
mapBase[my*mapX+mx]->Decay(logOddOccupiedFixedThr, logOddFreeFixedThr, r);
prevDecayT = t;
}
}
double resolution;
double decayInterval;
int logOddOccupied;
int logOddFree;
int logOddOccupiedThr;
int logOddOccupiedFixedThr;
int logOddFreeThr;
int logOddFreeFixedThr;
bool updated;
int updateCounter;
vector<OccupancyGridList*> updateList;
double originX, originY, originZ;
int mapX, mapY;
int expandStep;
vector<OccupancyGridList*> mapBase;
vector<arma::colvec> pts;
};
#endif

26
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/mainpage.dox
Unescape View File

@ -1,26 +0,0 @@
/**
\mainpage
\htmlinclude manifest.html
\b multi_map_server is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/

2
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/msg/MultiOccupancyGrid.msg
Unescape View File

@ -1,2 +0,0 @@
nav_msgs/OccupancyGrid[] maps
geometry_msgs/Pose[] origins

2
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/msg/MultiSparseMap3D.msg
Unescape View File

@ -1,2 +0,0 @@
SparseMap3D[] maps
geometry_msgs/Pose[] origins

4
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/msg/SparseMap3D.msg
Unescape View File

@ -1,4 +0,0 @@
Header header
nav_msgs/MapMetaData info
VerticalOccupancyGridList[] lists

6
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/msg/VerticalOccupancyGridList.msg
Unescape View File

@ -1,6 +0,0 @@
float32 x
float32 y
int32[] upper
int32[] lower
int32[] mass

38
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/package.xml
Unescape View File

@ -1,38 +0,0 @@
<package>
<description>multi_map_server</description>
<name>multi_map_server</name>
<version>0.0.0</version>
<maintainer email="eeshaojie@todo.todo">Shaojie Shen</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/multi_map_server</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>laser_geometry</build_depend>
<build_depend>pose_utils</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>quadrotor_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>visualization_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>laser_geometry</run_depend>
<run_depend>pose_utils</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>quadrotor_msgs</run_depend>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
</export>
</package>

1
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/src/multi_map_server/__init__.py
Unescape View File

@ -1 +0,0 @@
#autogenerated by ROS python message generators

404
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiOccupancyGrid.py
Unescape View File

@ -1,404 +0,0 @@
"""autogenerated by genpy from multi_map_server/MultiOccupancyGrid.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
import geometry_msgs.msg
import nav_msgs.msg
import genpy
import std_msgs.msg
class MultiOccupancyGrid(genpy.Message):
_md5sum = "61e63a291f11a6b1796a1edf79f34f72"
_type = "multi_map_server/MultiOccupancyGrid"
_has_header = False #flag to mark the presence of a Header object
_full_text = """nav_msgs/OccupancyGrid[] maps
geometry_msgs/Pose[] origins
================================================================================
MSG: nav_msgs/OccupancyGrid
# This represents a 2-D grid map, in which each cell represents the probability of
# occupancy.
Header header
#MetaData for the map
MapMetaData info
# The map data, in row-major order, starting with (0,0). Occupancy
# probabilities are in the range [0,100]. Unknown is -1.
int8[] data
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
================================================================================
MSG: nav_msgs/MapMetaData
# This hold basic information about the characterists of the OccupancyGrid
# The time at which the map was loaded
time map_load_time
# The map resolution [m/cell]
float32 resolution
# Map width [cells]
uint32 width
# Map height [cells]
uint32 height
# The origin of the map [m, m, rad]. This is the real-world pose of the
# cell (0,0) in the map.
geometry_msgs/Pose origin
================================================================================
MSG: geometry_msgs/Pose
# A representation of pose in free space, composed of postion and orientation.
Point position
Quaternion orientation
================================================================================
MSG: geometry_msgs/Point
# This contains the position of a point in free space
float64 x
float64 y
float64 z
================================================================================
MSG: geometry_msgs/Quaternion
# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w
"""
__slots__ = ['maps','origins']
_slot_types = ['nav_msgs/OccupancyGrid[]','geometry_msgs/Pose[]']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
maps,origins
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(MultiOccupancyGrid, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.maps is None:
self.maps = []
if self.origins is None:
self.origins = []
else:
self.maps = []
self.origins = []
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
length = len(self.maps)
buff.write(_struct_I.pack(length))
for val1 in self.maps:
_v1 = val1.header
buff.write(_struct_I.pack(_v1.seq))
_v2 = _v1.stamp
_x = _v2
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
_x = _v1.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_v3 = val1.info
_v4 = _v3.map_load_time
_x = _v4
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
_x = _v3
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
_v5 = _v3.origin
_v6 = _v5.position
_x = _v6
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
_v7 = _v5.orientation
_x = _v7
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
length = len(val1.data)
buff.write(_struct_I.pack(length))
pattern = '<%sb'%length
buff.write(struct.pack(pattern, *val1.data))
length = len(self.origins)
buff.write(_struct_I.pack(length))
for val1 in self.origins:
_v8 = val1.position
_x = _v8
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
_v9 = val1.orientation
_x = _v9
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
if self.maps is None:
self.maps = None
if self.origins is None:
self.origins = None
end = 0
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
self.maps = []
for i in range(0, length):
val1 = nav_msgs.msg.OccupancyGrid()
_v10 = val1.header
start = end
end += 4
(_v10.seq,) = _struct_I.unpack(str[start:end])
_v11 = _v10.stamp
_x = _v11
start = end
end += 8
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
_v10.frame_id = str[start:end].decode('utf-8')
else:
_v10.frame_id = str[start:end]
_v12 = val1.info
_v13 = _v12.map_load_time
_x = _v13
start = end
end += 8
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
_x = _v12
start = end
end += 12
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
_v14 = _v12.origin
_v15 = _v14.position
_x = _v15
start = end
end += 24
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
_v16 = _v14.orientation
_x = _v16
start = end
end += 32
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%sb'%length
start = end
end += struct.calcsize(pattern)
val1.data = struct.unpack(pattern, str[start:end])
self.maps.append(val1)
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
self.origins = []
for i in range(0, length):
val1 = geometry_msgs.msg.Pose()
_v17 = val1.position
_x = _v17
start = end
end += 24
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
_v18 = val1.orientation
_x = _v18
start = end
end += 32
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
self.origins.append(val1)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
length = len(self.maps)
buff.write(_struct_I.pack(length))
for val1 in self.maps:
_v19 = val1.header
buff.write(_struct_I.pack(_v19.seq))
_v20 = _v19.stamp
_x = _v20
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
_x = _v19.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_v21 = val1.info
_v22 = _v21.map_load_time
_x = _v22
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
_x = _v21
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
_v23 = _v21.origin
_v24 = _v23.position
_x = _v24
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
_v25 = _v23.orientation
_x = _v25
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
length = len(val1.data)
buff.write(_struct_I.pack(length))
pattern = '<%sb'%length
buff.write(val1.data.tostring())
length = len(self.origins)
buff.write(_struct_I.pack(length))
for val1 in self.origins:
_v26 = val1.position
_x = _v26
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
_v27 = val1.orientation
_x = _v27
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
if self.maps is None:
self.maps = None
if self.origins is None:
self.origins = None
end = 0
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
self.maps = []
for i in range(0, length):
val1 = nav_msgs.msg.OccupancyGrid()
_v28 = val1.header
start = end
end += 4
(_v28.seq,) = _struct_I.unpack(str[start:end])
_v29 = _v28.stamp
_x = _v29
start = end
end += 8
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
_v28.frame_id = str[start:end].decode('utf-8')
else:
_v28.frame_id = str[start:end]
_v30 = val1.info
_v31 = _v30.map_load_time
_x = _v31
start = end
end += 8
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
_x = _v30
start = end
end += 12
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
_v32 = _v30.origin
_v33 = _v32.position
_x = _v33
start = end
end += 24
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
_v34 = _v32.orientation
_x = _v34
start = end
end += 32
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%sb'%length
start = end
end += struct.calcsize(pattern)
val1.data = numpy.frombuffer(str[start:end], dtype=numpy.int8, count=length)
self.maps.append(val1)
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
self.origins = []
for i in range(0, length):
val1 = geometry_msgs.msg.Pose()
_v35 = val1.position
_x = _v35
start = end
end += 24
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
_v36 = val1.orientation
_x = _v36
start = end
end += 32
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
self.origins.append(val1)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_4d = struct.Struct("<4d")
_struct_f2I = struct.Struct("<f2I")
_struct_2I = struct.Struct("<2I")
_struct_3d = struct.Struct("<3d")

484
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiSparseMap3D.py
Unescape View File

@ -1,484 +0,0 @@
"""autogenerated by genpy from multi_map_server/MultiSparseMap3D.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
import geometry_msgs.msg
import multi_map_server.msg
import nav_msgs.msg
import genpy
import std_msgs.msg
class MultiSparseMap3D(genpy.Message):
_md5sum = "2e3d76c98ee3e2b23a422f64965f6418"
_type = "multi_map_server/MultiSparseMap3D"
_has_header = False #flag to mark the presence of a Header object
_full_text = """SparseMap3D[] maps
geometry_msgs/Pose[] origins
================================================================================
MSG: multi_map_server/SparseMap3D
Header header
nav_msgs/MapMetaData info
VerticalOccupancyGridList[] lists
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
================================================================================
MSG: nav_msgs/MapMetaData
# This hold basic information about the characterists of the OccupancyGrid
# The time at which the map was loaded
time map_load_time
# The map resolution [m/cell]
float32 resolution
# Map width [cells]
uint32 width
# Map height [cells]
uint32 height
# The origin of the map [m, m, rad]. This is the real-world pose of the
# cell (0,0) in the map.
geometry_msgs/Pose origin
================================================================================
MSG: geometry_msgs/Pose
# A representation of pose in free space, composed of postion and orientation.
Point position
Quaternion orientation
================================================================================
MSG: geometry_msgs/Point
# This contains the position of a point in free space
float64 x
float64 y
float64 z
================================================================================
MSG: geometry_msgs/Quaternion
# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w
================================================================================
MSG: multi_map_server/VerticalOccupancyGridList
float32 x
float32 y
int32[] upper
int32[] lower
int32[] mass
"""
__slots__ = ['maps','origins']
_slot_types = ['multi_map_server/SparseMap3D[]','geometry_msgs/Pose[]']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
maps,origins
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(MultiSparseMap3D, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.maps is None:
self.maps = []
if self.origins is None:
self.origins = []
else:
self.maps = []
self.origins = []
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
length = len(self.maps)
buff.write(_struct_I.pack(length))
for val1 in self.maps:
_v1 = val1.header
buff.write(_struct_I.pack(_v1.seq))
_v2 = _v1.stamp
_x = _v2
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
_x = _v1.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_v3 = val1.info
_v4 = _v3.map_load_time
_x = _v4
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
_x = _v3
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
_v5 = _v3.origin
_v6 = _v5.position
_x = _v6
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
_v7 = _v5.orientation
_x = _v7
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
length = len(val1.lists)
buff.write(_struct_I.pack(length))
for val2 in val1.lists:
_x = val2
buff.write(_struct_2f.pack(_x.x, _x.y))
length = len(val2.upper)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(struct.pack(pattern, *val2.upper))
length = len(val2.lower)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(struct.pack(pattern, *val2.lower))
length = len(val2.mass)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(struct.pack(pattern, *val2.mass))
length = len(self.origins)
buff.write(_struct_I.pack(length))
for val1 in self.origins:
_v8 = val1.position
_x = _v8
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
_v9 = val1.orientation
_x = _v9
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
if self.maps is None:
self.maps = None
if self.origins is None:
self.origins = None
end = 0
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
self.maps = []
for i in range(0, length):
val1 = multi_map_server.msg.SparseMap3D()
_v10 = val1.header
start = end
end += 4
(_v10.seq,) = _struct_I.unpack(str[start:end])
_v11 = _v10.stamp
_x = _v11
start = end
end += 8
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
_v10.frame_id = str[start:end].decode('utf-8')
else:
_v10.frame_id = str[start:end]
_v12 = val1.info
_v13 = _v12.map_load_time
_x = _v13
start = end
end += 8
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
_x = _v12
start = end
end += 12
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
_v14 = _v12.origin
_v15 = _v14.position
_x = _v15
start = end
end += 24
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
_v16 = _v14.orientation
_x = _v16
start = end
end += 32
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
val1.lists = []
for i in range(0, length):
val2 = multi_map_server.msg.VerticalOccupancyGridList()
_x = val2
start = end
end += 8
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val2.upper = struct.unpack(pattern, str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val2.lower = struct.unpack(pattern, str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val2.mass = struct.unpack(pattern, str[start:end])
val1.lists.append(val2)
self.maps.append(val1)
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
self.origins = []
for i in range(0, length):
val1 = geometry_msgs.msg.Pose()
_v17 = val1.position
_x = _v17
start = end
end += 24
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
_v18 = val1.orientation
_x = _v18
start = end
end += 32
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
self.origins.append(val1)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
length = len(self.maps)
buff.write(_struct_I.pack(length))
for val1 in self.maps:
_v19 = val1.header
buff.write(_struct_I.pack(_v19.seq))
_v20 = _v19.stamp
_x = _v20
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
_x = _v19.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_v21 = val1.info
_v22 = _v21.map_load_time
_x = _v22
buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
_x = _v21
buff.write(_struct_f2I.pack(_x.resolution, _x.width, _x.height))
_v23 = _v21.origin
_v24 = _v23.position
_x = _v24
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
_v25 = _v23.orientation
_x = _v25
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
length = len(val1.lists)
buff.write(_struct_I.pack(length))
for val2 in val1.lists:
_x = val2
buff.write(_struct_2f.pack(_x.x, _x.y))
length = len(val2.upper)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(val2.upper.tostring())
length = len(val2.lower)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(val2.lower.tostring())
length = len(val2.mass)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(val2.mass.tostring())
length = len(self.origins)
buff.write(_struct_I.pack(length))
for val1 in self.origins:
_v26 = val1.position
_x = _v26
buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
_v27 = val1.orientation
_x = _v27
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
if self.maps is None:
self.maps = None
if self.origins is None:
self.origins = None
end = 0
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
self.maps = []
for i in range(0, length):
val1 = multi_map_server.msg.SparseMap3D()
_v28 = val1.header
start = end
end += 4
(_v28.seq,) = _struct_I.unpack(str[start:end])
_v29 = _v28.stamp
_x = _v29
start = end
end += 8
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
_v28.frame_id = str[start:end].decode('utf-8')
else:
_v28.frame_id = str[start:end]
_v30 = val1.info
_v31 = _v30.map_load_time
_x = _v31
start = end
end += 8
(_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
_x = _v30
start = end
end += 12
(_x.resolution, _x.width, _x.height,) = _struct_f2I.unpack(str[start:end])
_v32 = _v30.origin
_v33 = _v32.position
_x = _v33
start = end
end += 24
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
_v34 = _v32.orientation
_x = _v34
start = end
end += 32
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
val1.lists = []
for i in range(0, length):
val2 = multi_map_server.msg.VerticalOccupancyGridList()
_x = val2
start = end
end += 8
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val2.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val2.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val2.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
val1.lists.append(val2)
self.maps.append(val1)
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
self.origins = []
for i in range(0, length):
val1 = geometry_msgs.msg.Pose()
_v35 = val1.position
_x = _v35
start = end
end += 24
(_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
_v36 = val1.orientation
_x = _v36
start = end
end += 32
(_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
self.origins.append(val1)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_f2I = struct.Struct("<f2I")
_struct_2f = struct.Struct("<2f")
_struct_4d = struct.Struct("<4d")
_struct_2I = struct.Struct("<2I")
_struct_3d = struct.Struct("<3d")

340
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_SparseMap3D.py
Unescape View File

@ -1,340 +0,0 @@
"""autogenerated by genpy from multi_map_server/SparseMap3D.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
import geometry_msgs.msg
import multi_map_server.msg
import nav_msgs.msg
import genpy
import std_msgs.msg
class SparseMap3D(genpy.Message):
_md5sum = "a20102f0b3a02e95070dab4140b78fb5"
_type = "multi_map_server/SparseMap3D"
_has_header = True #flag to mark the presence of a Header object
_full_text = """Header header
nav_msgs/MapMetaData info
VerticalOccupancyGridList[] lists
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
================================================================================
MSG: nav_msgs/MapMetaData
# This hold basic information about the characterists of the OccupancyGrid
# The time at which the map was loaded
time map_load_time
# The map resolution [m/cell]
float32 resolution
# Map width [cells]
uint32 width
# Map height [cells]
uint32 height
# The origin of the map [m, m, rad]. This is the real-world pose of the
# cell (0,0) in the map.
geometry_msgs/Pose origin
================================================================================
MSG: geometry_msgs/Pose
# A representation of pose in free space, composed of postion and orientation.
Point position
Quaternion orientation
================================================================================
MSG: geometry_msgs/Point
# This contains the position of a point in free space
float64 x
float64 y
float64 z
================================================================================
MSG: geometry_msgs/Quaternion
# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w
================================================================================
MSG: multi_map_server/VerticalOccupancyGridList
float32 x
float32 y
int32[] upper
int32[] lower
int32[] mass
"""
__slots__ = ['header','info','lists']
_slot_types = ['std_msgs/Header','nav_msgs/MapMetaData','multi_map_server/VerticalOccupancyGridList[]']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
header,info,lists
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(SparseMap3D, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.header is None:
self.header = std_msgs.msg.Header()
if self.info is None:
self.info = nav_msgs.msg.MapMetaData()
if self.lists is None:
self.lists = []
else:
self.header = std_msgs.msg.Header()
self.info = nav_msgs.msg.MapMetaData()
self.lists = []
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_2If2I7d.pack(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w))
length = len(self.lists)
buff.write(_struct_I.pack(length))
for val1 in self.lists:
_x = val1
buff.write(_struct_2f.pack(_x.x, _x.y))
length = len(val1.upper)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(struct.pack(pattern, *val1.upper))
length = len(val1.lower)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(struct.pack(pattern, *val1.lower))
length = len(val1.mass)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(struct.pack(pattern, *val1.mass))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
if self.info is None:
self.info = nav_msgs.msg.MapMetaData()
if self.lists is None:
self.lists = None
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 76
(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w,) = _struct_2If2I7d.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
self.lists = []
for i in range(0, length):
val1 = multi_map_server.msg.VerticalOccupancyGridList()
_x = val1
start = end
end += 8
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val1.upper = struct.unpack(pattern, str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val1.lower = struct.unpack(pattern, str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val1.mass = struct.unpack(pattern, str[start:end])
self.lists.append(val1)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_2If2I7d.pack(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w))
length = len(self.lists)
buff.write(_struct_I.pack(length))
for val1 in self.lists:
_x = val1
buff.write(_struct_2f.pack(_x.x, _x.y))
length = len(val1.upper)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(val1.upper.tostring())
length = len(val1.lower)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(val1.lower.tostring())
length = len(val1.mass)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(val1.mass.tostring())
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
if self.info is None:
self.info = nav_msgs.msg.MapMetaData()
if self.lists is None:
self.lists = None
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 76
(_x.info.map_load_time.secs, _x.info.map_load_time.nsecs, _x.info.resolution, _x.info.width, _x.info.height, _x.info.origin.position.x, _x.info.origin.position.y, _x.info.origin.position.z, _x.info.origin.orientation.x, _x.info.origin.orientation.y, _x.info.origin.orientation.z, _x.info.origin.orientation.w,) = _struct_2If2I7d.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
self.lists = []
for i in range(0, length):
val1 = multi_map_server.msg.VerticalOccupancyGridList()
_x = val1
start = end
end += 8
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val1.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val1.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
val1.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
self.lists.append(val1)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_3I = struct.Struct("<3I")
_struct_2f = struct.Struct("<2f")
_struct_2If2I7d = struct.Struct("<2If2I7d")

185
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_VerticalOccupancyGridList.py
Unescape View File

@ -1,185 +0,0 @@
"""autogenerated by genpy from multi_map_server/VerticalOccupancyGridList.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
class VerticalOccupancyGridList(genpy.Message):
_md5sum = "7ef85cc95b82747f51eb01a16bd7c795"
_type = "multi_map_server/VerticalOccupancyGridList"
_has_header = False #flag to mark the presence of a Header object
_full_text = """float32 x
float32 y
int32[] upper
int32[] lower
int32[] mass
"""
__slots__ = ['x','y','upper','lower','mass']
_slot_types = ['float32','float32','int32[]','int32[]','int32[]']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
x,y,upper,lower,mass
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(VerticalOccupancyGridList, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.x is None:
self.x = 0.
if self.y is None:
self.y = 0.
if self.upper is None:
self.upper = []
if self.lower is None:
self.lower = []
if self.mass is None:
self.mass = []
else:
self.x = 0.
self.y = 0.
self.upper = []
self.lower = []
self.mass = []
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
_x = self
buff.write(_struct_2f.pack(_x.x, _x.y))
length = len(self.upper)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(struct.pack(pattern, *self.upper))
length = len(self.lower)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(struct.pack(pattern, *self.lower))
length = len(self.mass)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(struct.pack(pattern, *self.mass))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
end = 0
_x = self
start = end
end += 8
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
self.upper = struct.unpack(pattern, str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
self.lower = struct.unpack(pattern, str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
self.mass = struct.unpack(pattern, str[start:end])
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
_x = self
buff.write(_struct_2f.pack(_x.x, _x.y))
length = len(self.upper)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(self.upper.tostring())
length = len(self.lower)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(self.lower.tostring())
length = len(self.mass)
buff.write(_struct_I.pack(length))
pattern = '<%si'%length
buff.write(self.mass.tostring())
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
end = 0
_x = self
start = end
end += 8
(_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
self.upper = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
self.lower = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
pattern = '<%si'%length
start = end
end += struct.calcsize(pattern)
self.mass = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_2f = struct.Struct("<2f")

4
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/__init__.py
Unescape View File

@ -1,4 +0,0 @@
from ._SparseMap3D import *
from ._MultiOccupancyGrid import *
from ._MultiSparseMap3D import *
from ._VerticalOccupancyGridList import *

90
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/src/multi_map_visualization.cc
Unescape View File

@ -1,90 +0,0 @@
#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <pose_utils.h>
#include <multi_map_server/MultiOccupancyGrid.h>
#include <multi_map_server/MultiSparseMap3D.h>
#include <multi_map_server/Map2D.h>
#include <multi_map_server/Map3D.h>
ros::Publisher pub1;
ros::Publisher pub2;
vector<Map2D> maps2d;
vector<geometry_msgs::Pose> origins2d;
vector<Map3D> maps3d;
vector<geometry_msgs::Pose> origins3d;
void maps2d_callback(const multi_map_server::MultiOccupancyGrid::ConstPtr &msg)
{
// Merge map
maps2d.resize(msg->maps.size(), Map2D(4));
for (unsigned int k = 0; k < msg->maps.size(); k++)
maps2d[k].Replace(msg->maps[k]);
origins2d = msg->origins;
// Assemble and publish map
multi_map_server::MultiOccupancyGrid m;
m.maps.resize(maps2d.size());
m.origins.resize(maps2d.size());
for (unsigned int k = 0; k < maps2d.size(); k++)
{
m.maps[k] = maps2d[k].GetMap();
m.origins[k] = origins2d[k];
}
pub1.publish(m);
}
void maps3d_callback(const multi_map_server::MultiSparseMap3D::ConstPtr &msg)
{
// Update incremental map
maps3d.resize(msg->maps.size());
for (unsigned int k = 0; k < msg->maps.size(); k++)
maps3d[k].UnpackMsg(msg->maps[k]);
origins3d = msg->origins;
// Publish
sensor_msgs::PointCloud m;
for (unsigned int k = 0; k < msg->maps.size(); k++)
{
colvec po(6);
po(0) = origins3d[k].position.x;
po(1) = origins3d[k].position.y;
po(2) = origins3d[k].position.z;
colvec poq(4);
poq(0) = origins3d[k].orientation.w;
poq(1) = origins3d[k].orientation.x;
poq(2) = origins3d[k].orientation.y;
poq(3) = origins3d[k].orientation.z;
po.rows(3,5) = R_to_ypr(quaternion_to_R(poq));
colvec tpo = po.rows(0,2);
mat Rpo = ypr_to_R(po.rows(3,5));
vector<colvec> pts = maps3d[k].GetOccupancyWorldFrame(OCCUPIED);
for (unsigned int i = 0; i < pts.size(); i++)
{
colvec pt = Rpo * pts[i] + tpo;
geometry_msgs::Point32 _pt;
_pt.x = pt(0);
_pt.y = pt(1);
_pt.z = pt(2);
m.points.push_back(_pt);
}
}
// Publish
m.header.stamp = ros::Time::now();
m.header.frame_id = string("/map");
pub2.publish(m);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "multi_map_visualization");
ros::NodeHandle n("~");
ros::Subscriber sub1 = n.subscribe("dmaps2d", 1, maps2d_callback);
ros::Subscriber sub2 = n.subscribe("dmaps3d", 1, maps3d_callback);
pub1 = n.advertise<multi_map_server::MultiOccupancyGrid>("maps2d", 1, true);
pub2 = n.advertise<sensor_msgs::PointCloud>("map3d", 1, true);
ros::spin();
return 0;
}

192
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/multi_map_server/src/unused/multi_map_server.cc
Unescape View File

@ -1,192 +0,0 @@
#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/PoseArray.h>
#include <laser_geometry/laser_geometry.h>
#include <pose_utils.h>
#include <multi_map_server/MultiOccupancyGrid.h>
#include <multi_map_server/MultiSparseMap3D.h>
#include <multi_map_server/Map2D.h>
#include <multi_map_server/Map3D.h>
using namespace arma;
using namespace std;
#define MAX_MAP_CNT 25
ros::Publisher pub1;
ros::Publisher pub2;
// 2D Map
int maps2dCnt = 0;
Map2D maps2d[MAX_MAP_CNT];
map_server_3d_new::MultiOccupancyGrid grids2d;
// 3D Map
int maps3dCnt = 0;
Map3D maps3d[MAX_MAP_CNT];
// Map origin from UKF
vector<geometry_msgs::Pose> maps_origin;
void map2d_callback(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
// Update msg and publish
if (grids2d.maps.size() == 0)
{
// Init msg
grids2d.maps.push_back(*msg);
maps2dCnt++;
}
else if (grids2d.maps.back().info.map_load_time != msg->info.map_load_time)
{
// Add Costmap
nav_msgs::OccupancyGrid m;
maps2d[maps2dCnt-1].get_map(m);
mapMatcher.AddCostMap(m);
// Increase msg size
grids2d.maps.back().data.clear();
grids2d.maps.push_back(*msg);
maps2dCnt++;
}
else
{
grids2d.maps.back() = *msg;
}
pub1.publish(grids2d);
// Update internval map
maps2d[maps2dCnt-1].update(*msg);
}
void scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg)
{
// Get Map Status
static bool isSLAMValid = false;
if (msg->intensities[10])
{
if (!isSLAMValid)
{
maps3dCnt++;
mapLinks.push_back(zeros<colvec>(3));
}
isSLAMValid = true;
}
else
{
isSLAMValid = false;
return;
}
// Scan cnt
mapLinks.back()(2)++;
// Get Current Pose
colvec pose(6);
pose(0) = msg->intensities[0];
pose(1) = msg->intensities[1];
pose(2) = msg->intensities[2];
pose(3) = msg->intensities[3];
pose(4) = msg->intensities[4];
pose(5) = msg->intensities[5];
colvec pose2D(3);
pose2D(0) = msg->intensities[0];
pose2D(1) = msg->intensities[1];
pose2D(2) = msg->intensities[3];
double currFloor = msg->intensities[7];
double currLaserH = msg->intensities[8];
// Horizontal laser scans
sensor_msgs::LaserScan scan = *msg;
scan.intensities.clear();
laser_geometry::LaserProjection projector;
sensor_msgs::PointCloud scanCloud;
projector.projectLaser(scan, scanCloud);
mat scanMat(3, scanCloud.points.size());
// Get scan in body frame
for (int k = 0; k < scanCloud.points.size(); k++)
{
scanMat(0,k) = scanCloud.points[k].x;
scanMat(1,k) = scanCloud.points[k].y;
scanMat(2,k) = 0.215 - 0.09;
}
// Downsample Laser scan to map resolution
double resolution = maps3d[maps3dCnt-1].GetResolution();
double px = NUM_INF;
double py = NUM_INF;
int cnt = 0;
mat scanMatDown = zeros<mat>(3, scanMat.n_cols);
for (int k = 0; k < scanMat.n_cols; k++)
{
double x = scanMat(0,k);
double y = scanMat(1,k);
double dist = (x-px)*(x-px) + (y-py)*(y-py);
if (dist > resolution * resolution)
{
scanMatDown.col(cnt) = scanMat.col(k);
px = x;
py = y;
cnt++;
}
}
if (cnt > 0)
scanMat = scanMatDown.cols(0, cnt-1);
else
scanMat = scanMatDown.cols(0,cnt);
// Transform to map local frame
scanMat = ypr_to_R(pose.rows(3,5)) * scanMat + pose.rows(0,2)*ones<rowvec>(scanMat.n_cols);
// Update map
for (int k = 0; k < scanMat.n_cols; k++)
maps3d[maps3dCnt-1].SetOccupancyFromWorldFrame(scanMat(0,k), scanMat(1,k), scanMat(2,k), PROB_LASER_OCCUPIED);
// downward facing laser scans
if (currLaserH > 0)
{
colvec pt(3);
pt(0) = 0;
pt(1) = 0;
pt(2) = -currLaserH;
pt = ypr_to_R(pose.rows(3,5)) * pt + pose.rows(0,2);
double resolution = maps3d[maps3dCnt-1].GetResolution();
for (double x = -0.1; x <= 0.1; x+=resolution)
for (double y = -0.1; y <= 0.1; y+=resolution)
maps3d[maps3dCnt-1].SetOccupancyFromWorldFrame(pt(0)+x, pt(1)+y, pt(2), PROB_LASER_OCCUPIED);
}
// Floor Levels
maps3d[maps3dCnt-1].SetFloor(currFloor);
}
void maps_origin_callback(const geometry_msgs::PoseArray::ConstPtr &msg)
{
maps_origin = msg->poses;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "multi_map_server_3d");
ros::NodeHandle n("~");
ros::Subscriber sub1 = n.subscribe("dmap2d", 100, map2d_callback);
ros::Subscriber sub2 = n.subscribe("scan", 100, scan_callback);
ros::Subscriber sub3 = n.subscribe("/maps_origin", 100, maps_origin_callback);
pub1 = n.advertise<multi_map_server::MultiOccupancyGrid>("dmaps2d", 10, true);
pub2 = n.advertise<multi_map_server::MultiSparseMap3D>( "dmaps3d", 10, true);
ros::Rate r(100.0);
int cnt = 0;
while (n.ok())
{
cnt++;
if (cnt > 100)
{
cnt = 0;
map_server_3d_new::MultiSparseMap3D msg;
msg.maps_origin = maps_origin;
for (int k = 0; k < maps3dCnt; k++)
{
msg.maps_active.push_back((bool)(!mapLinks[k](0)) && mapLinks[k](2) > 50);
map_server_3d_new::SparseMap3D m;
maps3d[k].GetSparseMap3DMsg(m);
m.header.seq = k;
msg.maps.push_back(m);
}
pub2.publish(msg);
}
ros::spinOnce();
r.sleep();
}
return 0;
}

213
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/odom_visualization/CMakeLists.txt
Unescape View File

@ -1,213 +0,0 @@
#cmake_minimum_required(VERSION 2.4.6)
#include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
#rosbuild_init()
#set the default path for built executables to the "bin" directory
#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
#rosbuild_add_executable(odom_visualization src/odom_visualization.cpp)
#target_link_libraries(odom_visualization pose_utils)
#----------------------------------
cmake_minimum_required(VERSION 2.8.3)
project(odom_visualization)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-O3 -Wall -g")
ADD_COMPILE_OPTIONS(-std=c++11 )
ADD_COMPILE_OPTIONS(-std=c++14 )
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
sensor_msgs
nav_msgs
visualization_msgs
quadrotor_msgs
tf
pose_utils
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependencies might have been
## pulled in transitively but can be declared for certainty nonetheless:
## * add a build_depend tag for "message_generation"
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
#add_message_files(
# FILES
# MultiOccupancyGrid.msg
# MultiSparseMap3D.msg
# SparseMap3D.msg
# VerticalOccupancyGridList.msg
#plan_cmd.msg
#)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
#generate_messages(
# DEPENDENCIES
# geometry_msgs
# nav_msgs
#)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES irobot_msgs
# CATKIN_DEPENDS geometry_msgs nav_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
find_package(Armadillo REQUIRED)
include_directories(${ARMADILLO_INCLUDE_DIRS})
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
## Declare a cpp library
# add_library(irobot_msgs
# src/${PROJECT_NAME}/irobot_msgs.cpp
# )
## Declare a cpp executable
add_executable(odom_visualization src/odom_visualization.cpp)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(multi_map_visualization multi_map_server_messages_cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(odom_visualization
${catkin_LIBRARIES}
${ARMADILLO_LIBRARIES}
${pose_utils_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS irobot_msgs irobot_msgs_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_irobot_msgs.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

1
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/odom_visualization/Makefile
Unescape View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

26
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/odom_visualization/mainpage.dox
Unescape View File

@ -1,26 +0,0 @@
/**
\mainpage
\htmlinclude manifest.html
\b odom_visualization is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/

BIN
src/UAV/Modules/ego_planner/ego-planner-swarm/src/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh
View File

Binary file not shown.

Some files were not shown because too many files have changed in this diff Show More

Write Preview
Loading…
Cancel
Save