diff --git a/README.en.md b/README.en.md index 397bcfe..3f934f0 100644 --- a/README.en.md +++ b/README.en.md @@ -22,53 +22,57 @@ Xiao, K., Ma, L., Tan, S., Cong, Y., Wang, X.: Implementation of UAV Coordinatio Developers can quickly verify algorithms with XTDrone, such as: -1. Stereo SLAM +Stereo SLAM -2. VIO +VIO -3. Dense Reconstruction +Dense Reconstruction -4. 2D Laser SLAM +2D Laser SLAM -5. 3D Laser SLAM +3D Laser SLAM -6. Motion Planning +2D Motion Planning - + -7. Object Detection and Tracking +3D Motion Planning + + + +Object Detection and Tracking -8. Formation +Formation -9. Fixed wing +Fixed wing -10. VTOL +VTOL -11. UGV +UGV -12. USV +USV diff --git a/README.md b/README.md index e422bb1..3664bf9 100644 --- a/README.md +++ b/README.md @@ -21,54 +21,58 @@ Xiao, K., Ma, L., Tan, S., Cong, Y., Wang, X.: Implementation of UAV Coordinatio 在这个平台上，开发者可以快速验证算法。如： -1. 双目SLAM +双目SLAM -2. 视觉惯性导航 +视觉惯性导航 -3. 三维稠密重建 +三维稠密重建 -4. 2D激光SLAM +2D激光SLAM -5. 3D激光SLAM +3D激光SLAM -6. 运动规划 +2D运动规划 - + -7. 目标检测与追踪 +3D运动规划 + + + +目标检测与追踪 -8. 多机协同 +多机协同 -9. 固定翼 +固定翼 -10. 复合翼 +复合翼 -11. 无人车 +无人车 -12. 无人船 +无人船 diff --git a/communication/multirotor_communication.py b/communication/multirotor_communication.py index 0c2a8d3..6843d0b 100644 --- a/communication/multirotor_communication.py +++ b/communication/multirotor_communication.py @@ -119,11 +119,15 @@ class Communication: def cmd_pose_flu_callback(self, msg): self.coordinate_frame = 9 - self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z) + self.motion_type = 0 + yaw = self.q2yaw(msg.orientation) + self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw) def cmd_pose_enu_callback(self, msg): self.coordinate_frame = 1 - self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z) + self.motion_type = 0 + yaw = self.q2yaw(msg.orientation) + self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw) def cmd_vel_flu_callback(self, msg): self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z) diff --git a/images/motion_planning.gif b/images/2d_motion_planning.gif similarity index 100% rename from images/motion_planning.gif rename to images/2d_motion_planning.gif diff --git a/images/3d_motion_planning.gif b/images/3d_motion_planning.gif new file mode 100755 index 0000000..1e03f71 Binary files /dev/null and b/images/3d_motion_planning.gif differ diff --git a/motion_planning/launch/2d_motion_planning.launch b/motion_planning/2d/launch/2d_motion_planning.launch similarity index 100% rename from motion_planning/launch/2d_motion_planning.launch rename to motion_planning/2d/launch/2d_motion_planning.launch diff --git a/motion_planning/launch/move_base.launch b/motion_planning/2d/launch/move_base.launch similarity index 100% rename from motion_planning/launch/move_base.launch rename to motion_planning/2d/launch/move_base.launch diff --git a/motion_planning/map/indoor3.pgm b/motion_planning/2d/map/indoor3.pgm similarity index 100% rename from motion_planning/map/indoor3.pgm rename to motion_planning/2d/map/indoor3.pgm diff --git a/motion_planning/map/indoor3.yaml b/motion_planning/2d/map/indoor3.yaml similarity index 100% rename from motion_planning/map/indoor3.yaml rename to motion_planning/2d/map/indoor3.yaml diff --git a/motion_planning/map/indoor5.pgm b/motion_planning/2d/map/indoor5.pgm similarity index 100% rename from motion_planning/map/indoor5.pgm rename to motion_planning/2d/map/indoor5.pgm diff --git a/motion_planning/map/indoor5.yaml b/motion_planning/2d/map/indoor5.yaml similarity index 100% rename from motion_planning/map/indoor5.yaml rename to motion_planning/2d/map/indoor5.yaml diff --git a/motion_planning/param/costmap_common_params.yaml b/motion_planning/2d/param/costmap_common_params.yaml similarity index 100% rename from motion_planning/param/costmap_common_params.yaml rename to motion_planning/2d/param/costmap_common_params.yaml diff --git a/motion_planning/param/dwa_local_planner_params.yaml b/motion_planning/2d/param/dwa_local_planner_params.yaml similarity index 100% rename from motion_planning/param/dwa_local_planner_params.yaml rename to motion_planning/2d/param/dwa_local_planner_params.yaml diff --git a/motion_planning/param/global_costmap_params.yaml b/motion_planning/2d/param/global_costmap_params.yaml similarity index 100% rename from motion_planning/param/global_costmap_params.yaml rename to motion_planning/2d/param/global_costmap_params.yaml diff --git a/motion_planning/param/global_planner_params.yaml b/motion_planning/2d/param/global_planner_params.yaml similarity index 100% rename from motion_planning/param/global_planner_params.yaml rename to motion_planning/2d/param/global_planner_params.yaml diff --git a/motion_planning/param/local_costmap_params.yaml b/motion_planning/2d/param/local_costmap_params.yaml similarity index 100% rename from motion_planning/param/local_costmap_params.yaml rename to motion_planning/2d/param/local_costmap_params.yaml diff --git a/motion_planning/param/move_base_params.yaml b/motion_planning/2d/param/move_base_params.yaml similarity index 100% rename from motion_planning/param/move_base_params.yaml rename to motion_planning/2d/param/move_base_params.yaml diff --git a/motion_planning/rviz/2d_motion_planning.rviz b/motion_planning/2d/rviz/2d_motion_planning.rviz similarity index 100% rename from motion_planning/rviz/2d_motion_planning.rviz rename to motion_planning/2d/rviz/2d_motion_planning.rviz diff --git a/motion_planning/3d/ego_planner/ego_rviz.rviz b/motion_planning/3d/ego_planner/ego_rviz.rviz new file mode 100644 index 0000000..a6d242c --- /dev/null +++ b/motion_planning/3d/ego_planner/ego_rviz.rviz @@ -0,0 +1,188 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 549 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /iris_0_ego_planner_node/optimal_list + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /iris_0_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /iris_0/mavros/vision_pose/pose + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /move_base_simple/goal + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 4.135247707366943 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.7557446956634521 + Y: 7.2424845695495605 + Z: 0.9154675006866455 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8753979206085205 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.0754048824310303 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 67 + Y: 30 diff --git a/motion_planning/3d/ego_planner/ego_transfer.py b/motion_planning/3d/ego_planner/ego_transfer.py new file mode 100755 index 0000000..9f1f753 --- /dev/null +++ b/motion_planning/3d/ego_planner/ego_transfer.py @@ -0,0 +1,34 @@ +import rospy +from geometry_msgs.msg import PoseStamped +import math +from pyquaternion import Quaternion +import tf +import sys + +vehicle_type = sys.argv[1] +local_pose = PoseStamped() +local_pose.header.frame_id = 'world' +quaternion = tf.transformations.quaternion_from_euler(-math.pi/2, 0, -math.pi/2) +q = Quaternion([quaternion[3],quaternion[0],quaternion[1],quaternion[2]]) + +def vision_callback(data): + local_pose.pose.position.x = data.pose.position.x + local_pose.pose.position.y = data.pose.position.y + local_pose.pose.position.z = data.pose.position.z + q_= Quaternion([data.pose.orientation.w,data.pose.orientation.x,data.pose.orientation.y,data.pose.orientation.z]) + q_ = q_*q + local_pose.pose.orientation.w = q_[0] + local_pose.pose.orientation.x = q_[1] + local_pose.pose.orientation.y = q_[2] + local_pose.pose.orientation.z = q_[3] + +rospy.init_node('ego_transfer') +rospy.Subscriber(vehicle_type+"_0/mavros/vision_pose/pose", PoseStamped, vision_callback) +position_pub = rospy.Publisher(vehicle_type+"_0/camera_pose", PoseStamped, queue_size=2) +rate = rospy.Rate(60) + +while True: + local_pose.header.stamp = rospy.Time.now() + position_pub.publish(local_pose) + rate.sleep() + diff --git a/motion_planning/3d/ego_planner/planner/bspline_opt/CMakeLists.txt b/motion_planning/3d/ego_planner/planner/bspline_opt/CMakeLists.txt new file mode 100755 index 0000000..e61c145 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/bspline_opt/CMakeLists.txt @@ -0,0 +1,45 @@ +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} + ) diff --git a/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/bspline_optimizer.h b/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/bspline_optimizer.h new file mode 100644 index 0000000..32abae5 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/bspline_optimizer.h @@ -0,0 +1,212 @@ +#ifndef _BSPLINE_OPTIMIZER_H_ +#define _BSPLINE_OPTIMIZER_H_ + +#include +#include +#include +#include +#include +#include +#include "bspline_opt/lbfgs.hpp" +#include + +// 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> base_point; // The point at the statrt of the direction vector (collision point) + std::vector> direction; // Direction vector, must be normalized. + std::vector flag_temp; // A flag that used in many places. Initialize it everytime before using it. + // std::vector 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 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 &guide_pt); + void setWaypoints(const vector &waypts, + const vector &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 ref_pts_; + + std::vector distinctiveTrajs(vector> segments); + std::vector> 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 guide_pts_; // geometric guiding path points, N-6 + vector waypoints_; // waypts constraints + vector 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 &x, std::vector &grad, void *func_data); + void combineCost(const std::vector &x, vector &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 Ptr; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + +} // namespace ego_planner +#endif \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h b/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h new file mode 100755 index 0000000..01a5a71 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/gradient_descent_optimizer.h @@ -0,0 +1,52 @@ +#ifndef _GRADIENT_DESCENT_OPT_H_ +#define _GRADIENT_DESCENT_OPT_H_ + +#include +#include +#include + +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 diff --git a/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/lbfgs.hpp b/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/lbfgs.hpp new file mode 100644 index 0000000..484c4b6 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/lbfgs.hpp @@ -0,0 +1,1451 @@ +#ifndef LBFGS_HPP +#define LBFGS_HPP + +#include +#include +#include +#include +#include + +namespace lbfgs +{ + // ----------------------- Data Type Part ----------------------- + + /** + * Return values of lbfgs_optimize(). + * + * Roughly speaking, a negative value indicates an error. + */ + + enum + { + /** L-BFGS reaches convergence. */ + LBFGS_CONVERGENCE = 0, + /** L-BFGS satisfies stopping criteria. */ + LBFGS_STOP, + /** The initial variables already minimize the objective function. */ + LBFGS_ALREADY_MINIMIZED, + + /** Unknown error. */ + LBFGSERR_UNKNOWNERROR = -1024, + /** Logic error. */ + LBFGSERR_LOGICERROR, + /** The minimization process has been canceled. */ + LBFGSERR_CANCELED, + /** Invalid number of variables specified. */ + LBFGSERR_INVALID_N, + /** Invalid parameter lbfgs_parameter_t::mem_size specified. */ + LBFGSERR_INVALID_MEMSIZE, + /** Invalid parameter lbfgs_parameter_t::g_epsilon specified. */ + LBFGSERR_INVALID_GEPSILON, + /** Invalid parameter lbfgs_parameter_t::past specified. */ + LBFGSERR_INVALID_TESTPERIOD, + /** Invalid parameter lbfgs_parameter_t::delta specified. */ + LBFGSERR_INVALID_DELTA, + /** Invalid parameter lbfgs_parameter_t::min_step specified. */ + LBFGSERR_INVALID_MINSTEP, + /** Invalid parameter lbfgs_parameter_t::max_step specified. */ + LBFGSERR_INVALID_MAXSTEP, + /** Invalid parameter lbfgs_parameter_t::f_dec_coeff specified. */ + LBFGSERR_INVALID_FDECCOEFF, + /** Invalid parameter lbfgs_parameter_t::wolfe specified. */ + LBFGSERR_INVALID_SCURVCOEFF, + /** Invalid parameter lbfgs_parameter_t::xtol specified. */ + LBFGSERR_INVALID_XTOL, + /** Invalid parameter lbfgs_parameter_t::max_linesearch specified. */ + LBFGSERR_INVALID_MAXLINESEARCH, + /** The line-search step went out of the interval of uncertainty. */ + LBFGSERR_OUTOFINTERVAL, + /** A logic error occurred; alternatively, the interval of uncertainty + became too small. */ + LBFGSERR_INCORRECT_TMINMAX, + /** A rounding error occurred; alternatively, no line-search step + satisfies the sufficient decrease and curvature conditions. */ + LBFGSERR_ROUNDING_ERROR, + /** The line-search step became smaller than lbfgs_parameter_t::min_step. */ + LBFGSERR_MINIMUMSTEP, + /** The line-search step became larger than lbfgs_parameter_t::max_step. */ + LBFGSERR_MAXIMUMSTEP, + /** The line-search routine reaches the maximum number of evaluations. */ + LBFGSERR_MAXIMUMLINESEARCH, + /** The algorithm routine reaches the maximum number of iterations. */ + LBFGSERR_MAXIMUMITERATION, + /** Relative width of the interval of uncertainty is at most + lbfgs_parameter_t::xtol. */ + LBFGSERR_WIDTHTOOSMALL, + /** A logic error (negative line-search step) occurred. */ + LBFGSERR_INVALIDPARAMETERS, + /** The current search direction increases the objective function value. */ + LBFGSERR_INCREASEGRADIENT, + }; + + /** + * L-BFGS optimization parameters. + * Call lbfgs_load_default_parameters() function to initialize parameters to the + * default values. + */ + struct lbfgs_parameter_t + { + /** + * The number of corrections to approximate the inverse hessian matrix. + * The L-BFGS routine stores the computation results of previous m + * iterations to approximate the inverse hessian matrix of the current + * iteration. This parameter controls the size of the limited memories + * (corrections). The default value is 6. Values less than 3 are + * not recommended. Large values will result in excessive computing time. + */ + int mem_size; + + /** + * Epsilon for grad norm convergence test. + * This parameter determines the accuracy with which the solution is to + * be found. A minimization terminates when + * ||g|| < g_epsilon * max(1, ||x||), + * where ||.|| denotes the Euclidean (L2) norm. The default value is 1e-5. + */ + double g_epsilon; + + /** + * Distance for delta-based convergence test. + * This parameter determines the distance, in iterations, to compute + * the rate of decrease of the objective function. If the value of this + * parameter is zero, the library does not perform the delta-based + * convergence test. The default value is 0. + */ + int past; + + /** + * Delta for convergence test. + * This parameter determines the minimum rate of decrease of the + * objective function. The library stops iterations when the + * following condition is met: + * (f' - f) / f < delta, + * where f' is the objective value of past iterations ago, and f is + * the objective value of the current iteration. + * The default value is 1e-5. + */ + double delta; + + /** + * The maximum number of iterations. + * The lbfgs_optimize() function terminates an optimization process with + * ::LBFGSERR_MAXIMUMITERATION status code when the iteration count + * exceedes this parameter. Setting this parameter to zero continues an + * optimization process until a convergence or error. The default value + * is 0. + */ + int max_iterations; + + /** + * The maximum number of trials for the line search. + * This parameter controls the number of function and gradients evaluations + * per iteration for the line search routine. The default value is 40. + */ + int max_linesearch; + + /** + * The minimum step of the line search routine. + * The default value is 1e-20. This value need not be modified unless + * the exponents are too large for the machine being used, or unless the + * problem is extremely badly scaled (in which case the exponents should + * be increased). + */ + double min_step; + + /** + * The maximum step of the line search. + * The default value is 1e+20. This value need not be modified unless + * the exponents are too large for the machine being used, or unless the + * problem is extremely badly scaled (in which case the exponents should + * be increased). + */ + double max_step; + + /** + * A parameter to control the accuracy of the line search routine. + * The default value is 1e-4. This parameter should be greater + * than zero and smaller than 0.5. + */ + double f_dec_coeff; + + /** + * A parameter to control the accuracy of the line search routine. + * The default value is 0.9. If the function and gradient + * evaluations are inexpensive with respect to the cost of the + * iteration (which is sometimes the case when solving very large + * problems) it may be advantageous to set this parameter to a small + * value. A typical small value is 0.1. This parameter shuold be + * greater than the f_dec_coeff parameter (1e-4) + * and smaller than 1.0. + */ + double s_curv_coeff; + + /** + * The machine precision for floating-point values. + * This parameter must be a positive value set by a client program to + * estimate the machine precision. The line search routine will terminate + * with the status code (::LBFGSERR_ROUNDING_ERROR) if the relative width + * of the interval of uncertainty is less than this parameter. + */ + double xtol; + }; + + /** + * Callback interface to provide objective function and gradient evaluations. + * + * The lbfgs_optimize() function call this function to obtain the values of objective + * function and its gradients when needed. A client program must implement + * this function to evaluate the values of the objective function and its + * gradients, given current values of variables. + * + * @param instance The user data sent for lbfgs_optimize() function by the client. + * @param x The current values of variables. + * @param g The gradient vector. The callback function must compute + * the gradient values for the current variables. + * @param n The number of variables. + * @retval double The value of the objective function for the current + * variables. + */ + typedef double (*lbfgs_evaluate_t)(void *instance, + const double *x, + double *g, + const int n); + + /** + * Callback interface to provide an upper bound at the beginning of current linear search. + * + * The lbfgs_optimize() function call this function to obtain the values of the + * upperbound of the stepsize to search in, provided with the beginning values of + * variables before the linear search, and the current step vector (can be descent direction). + * A client program can implement this function for more efficient linesearch. + * If it is not used, just set it NULL or nullptr. + * + * @param instance The user data sent for lbfgs_optimize() function by the client. + * @param xp The values of variables before current line search. + * @param d The step vector. It can be the descent direction. + * @param n The number of variables. + * @retval double The upperboud of the step in current line search routine, + * such that stpbound*d is the maximum reasonable step. + */ + typedef double (*lbfgs_stepbound_t)(void *instance, + const double *xp, + const double *d, + const int n); + + /** + * Callback interface to receive the progress of the optimization process. + * + * The lbfgs_optimize() function call this function for each iteration. Implementing + * this function, a client program can store or display the current progress + * of the optimization process. If it is not used, just set it NULL or nullptr. + * + * @param instance The user data sent for lbfgs_optimize() function by the client. + * @param x The current values of variables. + * @param g The current gradient values of variables. + * @param fx The current value of the objective function. + * @param xnorm The Euclidean norm of the variables. + * @param gnorm The Euclidean norm of the gradients. + * @param step The line-search step used for this iteration. + * @param n The number of variables. + * @param k The iteration count. + * @param ls The number of evaluations called for this iteration. + * @retval int Zero to continue the optimization process. Returning a + * non-zero value will cancel the optimization process. + */ + typedef int (*lbfgs_progress_t)(void *instance, + const double *x, + const double *g, + const double fx, + const double xnorm, + const double gnorm, + const double step, + int n, + int k, + int ls); + + /** + * Callback data struct + */ + + struct callback_data_t + { + int n; + void *instance; + lbfgs_evaluate_t proc_evaluate; + lbfgs_stepbound_t proc_stepbound; + lbfgs_progress_t proc_progress; + }; + + /** + * Iteration data struct + */ + struct iteration_data_t + { + double alpha; + double *s; /* [n] */ + double *y; /* [n] */ + double ys; /* vecdot(y, s) */ + }; + + // ----------------------- Arithmetic Part ----------------------- + +/** + * Define the local variables for computing minimizers. + */ +#define USES_MINIMIZER_LBFGS \ + double a, d, gamm, theta, p, q, r, s; + +/** + * Find a minimizer of an interpolated cubic function. + * @param cm The minimizer of the interpolated cubic. + * @param u The value of one point, u. + * @param fu The value of f(u). + * @param du The value of f'(u). + * @param v The value of another point, v. + * @param fv The value of f(v). + * @param du The value of f'(v). + */ +#define CUBIC_MINIMIZER_LBFGS(cm, u, fu, du, v, fv, dv) \ + d = (v) - (u); \ + theta = ((fu) - (fv)) * 3 / d + (du) + (dv); \ + p = fabs(theta); \ + q = fabs(du); \ + r = fabs(dv); \ + s = p >= q ? p : q; \ + s = s >= r ? s : r; \ + /* gamm = s*sqrt((theta/s)**2 - (du/s) * (dv/s)) */ \ + a = theta / s; \ + gamm = s * sqrt(a * a - ((du) / s) * ((dv) / s)); \ + if ((v) < (u)) \ + gamm = -gamm; \ + p = gamm - (du) + theta; \ + q = gamm - (du) + gamm + (dv); \ + r = p / q; \ + (cm) = (u) + r * d; + +/** + * Find a minimizer of an interpolated cubic function. + * @param cm The minimizer of the interpolated cubic. + * @param u The value of one point, u. + * @param fu The value of f(u). + * @param du The value of f'(u). + * @param v The value of another point, v. + * @param fv The value of f(v). + * @param du The value of f'(v). + * @param xmin The maximum value. + * @param xmin The minimum value. + */ +#define CUBIC_MINIMIZER2_LBFGS(cm, u, fu, du, v, fv, dv, xmin, xmax) \ + d = (v) - (u); \ + theta = ((fu) - (fv)) * 3 / d + (du) + (dv); \ + p = fabs(theta); \ + q = fabs(du); \ + r = fabs(dv); \ + s = p >= q ? p : q; \ + s = s >= r ? s : r; \ + /* gamm = s*sqrt((theta/s)**2 - (du/s) * (dv/s)) */ \ + a = theta / s; \ + gamm = a * a - ((du) / s) * ((dv) / s); \ + gamm = gamm > 0 ? s * sqrt(gamm) : 0; \ + if ((u) < (v)) \ + gamm = -gamm; \ + p = gamm - (dv) + theta; \ + q = gamm - (dv) + gamm + (du); \ + r = p / q; \ + if (r < 0. && gamm != 0.) \ + { \ + (cm) = (v)-r * d; \ + } \ + else if (a < 0) \ + { \ + (cm) = (xmax); \ + } \ + else \ + { \ + (cm) = (xmin); \ + } + +/** + * Find a minimizer of an interpolated quadratic function. + * @param qm The minimizer of the interpolated quadratic. + * @param u The value of one point, u. + * @param fu The value of f(u). + * @param du The value of f'(u). + * @param v The value of another point, v. + * @param fv The value of f(v). + */ +#define QUARD_MINIMIZER_LBFGS(qm, u, fu, du, v, fv) \ + a = (v) - (u); \ + (qm) = (u) + (du) / (((fu) - (fv)) / a + (du)) / 2 * a; + +/** + * Find a minimizer of an interpolated quadratic function. + * @param qm The minimizer of the interpolated quadratic. + * @param u The value of one point, u. + * @param du The value of f'(u). + * @param v The value of another point, v. + * @param dv The value of f'(v). + */ +#define QUARD_MINIMIZER2_LBFGS(qm, u, du, v, dv) \ + a = (u) - (v); \ + (qm) = (v) + (dv) / ((dv) - (du)) * a; + + inline void *vecalloc(size_t size) + { + void *memblock = malloc(size); + if (memblock) + { + memset(memblock, 0, size); + } + return memblock; + } + + inline void vecfree(void *memblock) + { + free(memblock); + } + + inline void veccpy(double *y, const double *x, const int n) + { + memcpy(y, x, sizeof(double) * n); + } + + inline void vecncpy(double *y, const double *x, const int n) + { + int i; + + for (i = 0; i < n; ++i) + { + y[i] = -x[i]; + } + } + + inline void vecadd(double *y, const double *x, const double c, const int n) + { + int i; + + for (i = 0; i < n; ++i) + { + y[i] += c * x[i]; + } + } + + inline void vecdiff(double *z, const double *x, const double *y, const int n) + { + int i; + + for (i = 0; i < n; ++i) + { + z[i] = x[i] - y[i]; + } + } + + inline void vecscale(double *y, const double c, const int n) + { + int i; + + for (i = 0; i < n; ++i) + { + y[i] *= c; + } + } + + inline void vecdot(double *s, const double *x, const double *y, const int n) + { + int i; + *s = 0.; + for (i = 0; i < n; ++i) + { + *s += x[i] * y[i]; + } + } + + inline void vec2norm(double *s, const double *x, const int n) + { + vecdot(s, x, x, n); + *s = (double)sqrt(*s); + } + + inline void vec2norminv(double *s, const double *x, const int n) + { + vec2norm(s, x, n); + *s = (double)(1.0 / *s); + } + + // ----------------------- L-BFGS Part ----------------------- + + /** + * Update a safeguarded trial value and interval for line search. + * + * The parameter x represents the step with the least function value. + * The parameter t represents the current step. This function assumes + * that the derivative at the point of x in the direction of the step. + * If the bracket is set to true, the minimizer has been bracketed in + * an interval of uncertainty with endpoints between x and y. + * + * @param x The pointer to the value of one endpoint. + * @param fx The pointer to the value of f(x). + * @param dx The pointer to the value of f'(x). + * @param y The pointer to the value of another endpoint. + * @param fy The pointer to the value of f(y). + * @param dy The pointer to the value of f'(y). + * @param t The pointer to the value of the trial value, t. + * @param ft The pointer to the value of f(t). + * @param dt The pointer to the value of f'(t). + * @param tmin The minimum value for the trial value, t. + * @param tmax The maximum value for the trial value, t. + * @param brackt The pointer to the predicate if the trial value is + * bracketed. + * @retval int Status value. Zero indicates a normal termination. + * + * @see + * Jorge J. More and David J. Thuente. Line search algorithm with + * guaranteed sufficient decrease. ACM Transactions on Mathematical + * Software (TOMS), Vol 20, No 3, pp. 286-307, 1994. + */ + inline int update_trial_interval(double *x, + double *fx, + double *dx, + double *y, + double *fy, + double *dy, + double *t, + double *ft, + double *dt, + const double tmin, + const double tmax, + int *brackt) + { + int bound; + int dsign = *dt * (*dx / fabs(*dx)) < 0.; + double mc; /* minimizer of an interpolated cubic. */ + double mq; /* minimizer of an interpolated quadratic. */ + double newt; /* new trial value. */ + USES_MINIMIZER_LBFGS; /* for CUBIC_MINIMIZER and QUARD_MINIMIZER. */ + + /* Check the input parameters for errors. */ + if (*brackt) + { + if (*t <= (*x <= *y ? *x : *y) || (*x >= *y ? *x : *y) <= *t) + { + /* The trival value t is out of the interval. */ + return LBFGSERR_OUTOFINTERVAL; + } + if (0. <= *dx * (*t - *x)) + { + /* The function must decrease from x. */ + return LBFGSERR_INCREASEGRADIENT; + } + if (tmax < tmin) + { + /* Incorrect tmin and tmax specified. */ + return LBFGSERR_INCORRECT_TMINMAX; + } + } + + /* + Trial value selection. + */ + if (*fx < *ft) + { + /* + Case 1: a higher function value. + The minimum is brackt. If the cubic minimizer is closer + to x than the quadratic one, the cubic one is taken, else + the average of the minimizers is taken. + */ + *brackt = 1; + bound = 1; + CUBIC_MINIMIZER_LBFGS(mc, *x, *fx, *dx, *t, *ft, *dt); + QUARD_MINIMIZER_LBFGS(mq, *x, *fx, *dx, *t, *ft); + if (fabs(mc - *x) < fabs(mq - *x)) + { + newt = mc; + } + else + { + newt = mc + 0.5 * (mq - mc); + } + } + else if (dsign) + { + /* + Case 2: a lower function value and derivatives of + opposite sign. The minimum is brackt. If the cubic + minimizer is closer to x than the quadratic (secant) one, + the cubic one is taken, else the quadratic one is taken. + */ + *brackt = 1; + bound = 0; + CUBIC_MINIMIZER_LBFGS(mc, *x, *fx, *dx, *t, *ft, *dt); + QUARD_MINIMIZER2_LBFGS(mq, *x, *dx, *t, *dt); + if (fabs(mc - *t) > fabs(mq - *t)) + { + newt = mc; + } + else + { + newt = mq; + } + } + else if (fabs(*dt) < fabs(*dx)) + { + /* + Case 3: a lower function value, derivatives of the + same sign, and the magnitude of the derivative decreases. + The cubic minimizer is only used if the cubic tends to + infinity in the direction of the minimizer or if the minimum + of the cubic is beyond t. Otherwise the cubic minimizer is + defined to be either tmin or tmax. The quadratic (secant) + minimizer is also computed and if the minimum is brackt + then the the minimizer closest to x is taken, else the one + farthest away is taken. + */ + bound = 1; + CUBIC_MINIMIZER2_LBFGS(mc, *x, *fx, *dx, *t, *ft, *dt, tmin, tmax); + QUARD_MINIMIZER2_LBFGS(mq, *x, *dx, *t, *dt); + if (*brackt) + { + if (fabs(*t - mc) < fabs(*t - mq)) + { + newt = mc; + } + else + { + newt = mq; + } + } + else + { + if (fabs(*t - mc) > fabs(*t - mq)) + { + newt = mc; + } + else + { + newt = mq; + } + } + } + else + { + /* + Case 4: a lower function value, derivatives of the + same sign, and the magnitude of the derivative does + not decrease. If the minimum is not brackt, the step + is either tmin or tmax, else the cubic minimizer is taken. + */ + bound = 0; + if (*brackt) + { + CUBIC_MINIMIZER_LBFGS(newt, *t, *ft, *dt, *y, *fy, *dy); + } + else if (*x < *t) + { + newt = tmax; + } + else + { + newt = tmin; + } + } + + /* + Update the interval of uncertainty. This update does not + depend on the new step or the case analysis above. + + - Case a: if f(x) < f(t), + x <- x, y <- t. + - Case b: if f(t) <= f(x) && f'(t)*f'(x) > 0, + x <- t, y <- y. + - Case c: if f(t) <= f(x) && f'(t)*f'(x) < 0, + x <- t, y <- x. + */ + if (*fx < *ft) + { + /* Case a */ + *y = *t; + *fy = *ft; + *dy = *dt; + } + else + { + /* Case c */ + if (dsign) + { + *y = *x; + *fy = *fx; + *dy = *dx; + } + /* Cases b and c */ + *x = *t; + *fx = *ft; + *dx = *dt; + } + + /* Clip the new trial value in [tmin, tmax]. */ + if (tmax < newt) + newt = tmax; + if (newt < tmin) + newt = tmin; + + /* + Redefine the new trial value if it is close to the upper bound + of the interval. + */ + if (*brackt && bound) + { + mq = *x + 0.66 * (*y - *x); + if (*x < *y) + { + if (mq < newt) + newt = mq; + } + else + { + if (newt < mq) + newt = mq; + } + } + + /* Return the new trial value. */ + *t = newt; + return 0; + } + + inline int line_search_morethuente(int n, + double *x, + double *f, + double *g, + double *s, + double *stp, + const double *xp, + const double *gp, + const double *stpmin, + const double *stpmax, + callback_data_t *cd, + const lbfgs_parameter_t *param) + { + int count = 0; + int brackt, stage1, uinfo = 0; + double dg; + double stx, fx, dgx; + double sty, fy, dgy; + double fxm, dgxm, fym, dgym, fm, dgm; + double finit, ftest1, dginit, dgtest; + double width, prev_width; + double stmin, stmax; + + /* Check the input parameters for errors. */ + if (*stp <= 0.) + { + return LBFGSERR_INVALIDPARAMETERS; + } + + /* Compute the initial gradient in the search direction. */ + vecdot(&dginit, g, s, n); + + /* Make sure that s points to a descent direction. */ + if (0 < dginit) + { + return LBFGSERR_INCREASEGRADIENT; + } + + /* Initialize local variables. */ + brackt = 0; + stage1 = 1; + finit = *f; + dgtest = param->f_dec_coeff * dginit; + width = *stpmax - *stpmin; + prev_width = 2.0 * width; + + /* + The variables stx, fx, dgx contain the values of the step, + function, and directional derivative at the best step. + The variables sty, fy, dgy contain the value of the step, + function, and derivative at the other endpoint of + the interval of uncertainty. + The variables stp, f, dg contain the values of the step, + function, and derivative at the current step. + */ + stx = sty = 0.; + fx = fy = finit; + dgx = dgy = dginit; + + for (;;) + { + /* Report the progress. */ + if (cd->proc_progress) + { + double xnorm; + double gnorm; + vec2norm(&xnorm, x, n); + vec2norm(&gnorm, g, n); + if (cd->proc_progress(cd->instance, x, g, fx, xnorm, gnorm, *stp, cd->n, 0, 0)) + { + return LBFGSERR_CANCELED; + } + } + + /* + Set the minimum and maximum steps to correspond to the + present interval of uncertainty. + */ + if (brackt) + { + stmin = stx <= sty ? stx : sty; + stmax = stx >= sty ? stx : sty; + } + else + { + stmin = stx; + stmax = *stp + 4.0 * (*stp - stx); + } + + /* Clip the step in the range of [stpmin, stpmax]. */ + if (*stp < *stpmin) + *stp = *stpmin; + if (*stpmax < *stp) + *stp = *stpmax; + + /* + If an unusual termination is to occur then let + stp be the lowest point obtained so far. + */ + if ((brackt && ((*stp <= stmin || stmax <= *stp) || param->max_linesearch <= count + 1 || uinfo != 0)) || (brackt && (stmax - stmin <= param->xtol * stmax))) + { + *stp = stx; + } + + /* + Compute the current value of x: + x <- x + (*stp) * s. + */ + veccpy(x, xp, n); + vecadd(x, s, *stp, n); + + /* Evaluate the function and gradient values. */ + *f = cd->proc_evaluate(cd->instance, x, g, cd->n); + vecdot(&dg, g, s, n); + + ftest1 = finit + *stp * dgtest; + ++count; + + /* Test for errors and convergence. */ + if (brackt && ((*stp <= stmin || stmax <= *stp) || uinfo != 0)) + { + /* Rounding errors prevent further progress. */ + return LBFGSERR_ROUNDING_ERROR; + } + if (*stp == *stpmax && *f <= ftest1 && dg <= dgtest) + { + /* The step is the maximum value. */ + return LBFGSERR_MAXIMUMSTEP; + } + if (*stp == *stpmin && (ftest1 < *f || dgtest <= dg)) + { + /* The step is the minimum value. */ + return LBFGSERR_MINIMUMSTEP; + } + if (brackt && (stmax - stmin) <= param->xtol * stmax) + { + /* Relative width of the interval of uncertainty is at most xtol. */ + return LBFGSERR_WIDTHTOOSMALL; + } + if (param->max_linesearch <= count) + { + /* Maximum number of iteration. */ + return LBFGSERR_MAXIMUMLINESEARCH; + } + if (*f <= ftest1 && fabs(dg) <= param->s_curv_coeff * (-dginit)) + { + /* The sufficient decrease condition and the strong curvature condition hold. */ + return count; + } + + /* + In the first stage we seek a step for which the modified + function has a nonpositive value and nonnegative derivative. + */ + if (stage1 && *f <= ftest1 && + (param->f_dec_coeff <= param->s_curv_coeff ? param->f_dec_coeff : param->s_curv_coeff) * dginit <= dg) + { + stage1 = 0; + } + + /* + A modified function is used to predict the step only if + we have not obtained a step for which the modified + function has a nonpositive function value and nonnegative + derivative, and if a lower function value has been + obtained but the decrease is not sufficient. + */ + if (stage1 && ftest1 < *f && *f <= fx) + { + /* Define the modified function and derivative values. */ + fm = *f - *stp * dgtest; + fxm = fx - stx * dgtest; + fym = fy - sty * dgtest; + dgm = dg - dgtest; + dgxm = dgx - dgtest; + dgym = dgy - dgtest; + + /* + Call update_trial_interval() to update the interval of + uncertainty and to compute the new step. + */ + uinfo = update_trial_interval( + &stx, &fxm, &dgxm, + &sty, &fym, &dgym, + stp, &fm, &dgm, + stmin, stmax, &brackt); + + /* Reset the function and gradient values for f. */ + fx = fxm + stx * dgtest; + fy = fym + sty * dgtest; + dgx = dgxm + dgtest; + dgy = dgym + dgtest; + } + else + { + /* + Call update_trial_interval() to update the interval of + uncertainty and to compute the new step. + */ + uinfo = update_trial_interval( + &stx, &fx, &dgx, + &sty, &fy, &dgy, + stp, f, &dg, + stmin, stmax, &brackt); + } + + /* + Force a sufficient decrease in the interval of uncertainty. + */ + if (brackt) + { + if (0.66 * prev_width <= fabs(sty - stx)) + { + *stp = stx + 0.5 * (sty - stx); + } + prev_width = width; + width = fabs(sty - stx); + } + } + + return LBFGSERR_LOGICERROR; + } + + /** + * Default L-BFGS parameters. + */ + static const lbfgs_parameter_t _default_param = { + 8, + 1e-5, + 0, + 1e-5, + 0, + 40, + 1e-20, + 1e20, + 1e-4, + 0.9, + 1.0e-16, + }; + + /** + * Initialize L-BFGS parameters to the default values. + * + * Call this function to fill a parameter structure with the default values + * and overwrite parameter values if necessary. + * + * @param param The pointer to the parameter structure. + */ + inline void lbfgs_load_default_parameters(lbfgs_parameter_t *param) + { + memcpy(param, &_default_param, sizeof(*param)); + } + + /** + * Start a L-BFGS optimization. + * A user must implement a function compatible with ::lbfgs_evaluate_t (evaluation + * callback) and pass the pointer to the callback function to lbfgs_optimize() + * arguments. Similarly, a user can implement a function compatible with + * ::lbfgs_stepbound_t to provide an external upper bound for stepsize, and + * ::lbfgs_progress_t (progress callback) to obtain the current progress + * (e.g., variables, function value, ||G||, etc) and to cancel the iteration + * process if necessary. Implementation of the stepbound and the progress callback + * is optional: a user can pass NULL if progress notification is not necessary. + * + * This algorithm terminates an optimization + * when: + * + * ||G|| < g_epsilon \cdot \max(1, ||x||) . + * + * In this formula, ||.|| denotes the Euclidean norm. + * + * @param n The number of variables. + * @param x The array of variables. A client program can set + * default values for the optimization and receive the + * optimization result through this array. + * @param ptr_fx The pointer to the variable that receives the final + * value of the objective function for the variables. + * This argument can be set to NULL if the final + * value of the objective function is unnecessary. + * @param proc_evaluate The callback function to provide function and + * gradient evaluations given a current values of + * variables. A client program must implement a + * callback function compatible with + * lbfgs_evaluate_t and pass the pointer to the + * callback function. + * @param proc_stepbound The callback function to provide values of the + * upperbound of the stepsize to search in, provided + * with the beginning values of variables before the + * linear search, and the current step vector (can + * be negative gradient). A client program can implement + * this function for more efficient linesearch. If it is + * not used, just set it NULL or nullptr. + * @param proc_progress The callback function to receive the progress + * (the number of iterations, the current value of + * the objective function) of the minimization + * process. This argument can be set to NULL if + * a progress report is unnecessary. + * @param instance A user data for the client program. The callback + * functions will receive the value of this argument. + * @param param The pointer to a structure representing parameters for + * L-BFGS optimization. A client program can set this + * parameter to NULL to use the default parameters. + * Call lbfgs_load_default_parameters() function to + * fill a structure with the default values. + * @retval int The status code. This function returns zero if the + * minimization process terminates without an error. A + * non-zero value indicates an error. + */ + inline int lbfgs_optimize(int n, + double *x, + double *ptr_fx, + lbfgs_evaluate_t proc_evaluate, + lbfgs_stepbound_t proc_stepbound, + lbfgs_progress_t proc_progress, + void *instance, + lbfgs_parameter_t *_param) + { + int ret; + int i, j, k, ls, end, bound; + double step; + int loop; + double step_min, step_max; + + /* Constant parameters and their default values. */ + lbfgs_parameter_t param = (_param != NULL) ? (*_param) : _default_param; + const int m = param.mem_size; + + double *xp = NULL; + double *g = NULL, *gp = NULL; + double *d = NULL, *pf = NULL; + iteration_data_t *lm = NULL, *it = NULL; + double ys, yy; + double xnorm, gnorm, beta; + double fx = 0.; + double rate = 0.; + + /* Construct a callback data. */ + callback_data_t cd; + cd.n = n; + cd.instance = instance; + cd.proc_evaluate = proc_evaluate; + cd.proc_stepbound = proc_stepbound; + cd.proc_progress = proc_progress; + + /* Check the input parameters for errors. */ + if (n <= 0) + { + return LBFGSERR_INVALID_N; + } + if (m <= 0) + { + return LBFGSERR_INVALID_MEMSIZE; + } + if (param.g_epsilon < 0.) + { + return LBFGSERR_INVALID_GEPSILON; + } + if (param.past < 0) + { + return LBFGSERR_INVALID_TESTPERIOD; + } + if (param.delta < 0.) + { + return LBFGSERR_INVALID_DELTA; + } + if (param.min_step < 0.) + { + return LBFGSERR_INVALID_MINSTEP; + } + if (param.max_step < param.min_step) + { + return LBFGSERR_INVALID_MAXSTEP; + } + if (param.f_dec_coeff < 0.) + { + return LBFGSERR_INVALID_FDECCOEFF; + } + if (param.s_curv_coeff <= param.f_dec_coeff || 1. <= param.s_curv_coeff) + { + return LBFGSERR_INVALID_SCURVCOEFF; + } + if (param.xtol < 0.) + { + return LBFGSERR_INVALID_XTOL; + } + if (param.max_linesearch <= 0) + { + return LBFGSERR_INVALID_MAXLINESEARCH; + } + + /* Allocate working space. */ + xp = (double *)vecalloc(n * sizeof(double)); + g = (double *)vecalloc(n * sizeof(double)); + gp = (double *)vecalloc(n * sizeof(double)); + d = (double *)vecalloc(n * sizeof(double)); + + /* Allocate limited memory storage. */ + lm = (iteration_data_t *)vecalloc(m * sizeof(iteration_data_t)); + + /* Initialize the limited memory. */ + for (i = 0; i < m; ++i) + { + it = &lm[i]; + it->alpha = 0; + it->ys = 0; + it->s = (double *)vecalloc(n * sizeof(double)); + it->y = (double *)vecalloc(n * sizeof(double)); + } + + /* Allocate an array for storing previous values of the objective function. */ + if (0 < param.past) + { + pf = (double *)vecalloc(param.past * sizeof(double)); + } + + /* Evaluate the function value and its gradient. */ + fx = cd.proc_evaluate(cd.instance, x, g, cd.n); + + /* Store the initial value of the objective function. */ + if (pf != NULL) + { + pf[0] = fx; + } + + /* + Compute the direction; + we assume the initial hessian matrix H_0 as the identity matrix. + */ + vecncpy(d, g, n); + + /* + Make sure that the initial variables are not a minimizer. + */ + vec2norm(&xnorm, x, n); + vec2norm(&gnorm, g, n); + + if (xnorm < 1.0) + xnorm = 1.0; + if (gnorm / xnorm <= param.g_epsilon) + { + ret = LBFGS_ALREADY_MINIMIZED; + } + else + { + /* Compute the initial step: + step = 1.0 / sqrt(vecdot(d, d, n)) + */ + vec2norminv(&step, d, n); + + k = 1; + end = 0; + loop = 1; + + while (loop == 1) + { + /* Store the current position and gradient vectors. */ + veccpy(xp, x, n); + veccpy(gp, g, n); + + // If the step bound can be provied dynamically, then apply it. + step_min = param.min_step; + step_max = param.max_step; + if (cd.proc_stepbound) + { + step_max = cd.proc_stepbound(cd.instance, xp, d, cd.n); + step_max = step_max < param.max_step ? step_max : param.max_step; + if (step >= step_max) + step = step_max / 2.0; + } + + /* Search for an optimal step. */ + ls = line_search_morethuente(n, x, &fx, g, d, &step, xp, gp, &step_min, &step_max, &cd, ¶m); + + if (ls < 0) + { + /* Revert to the previous point. */ + veccpy(x, xp, n); + veccpy(g, gp, n); + ret = ls; + loop = 0; + continue; + } + + /* Compute x and g norms. */ + vec2norm(&xnorm, x, n); + vec2norm(&gnorm, g, n); + + // /* Report the progress. */ + // if (cd.proc_progress) + // { + // if ((ret = cd.proc_progress(cd.instance, x, g, fx, xnorm, gnorm, step, cd.n, k, ls))) + // { + // loop = 0; + // continue; + // } + // } + + /* + Convergence test. + The criterion is given by the following formula: + |g(x)| / \max(1, |x|) < g_epsilon + */ + if (xnorm < 1.0) + xnorm = 1.0; + if (gnorm / xnorm <= param.g_epsilon) + { + /* Convergence. */ + ret = LBFGS_CONVERGENCE; + break; + } + + /* + Test for stopping criterion. + The criterion is given by the following formula: + |(f(past_x) - f(x))| / f(x) < \delta + */ + if (pf != NULL) + { + /* We don't test the stopping criterion while k < past. */ + if (param.past <= k) + { + /* Compute the relative improvement from the past. */ + rate = (pf[k % param.past] - fx) / fx; + + /* The stopping criterion. */ + if (fabs(rate) < param.delta) + { + ret = LBFGS_STOP; + break; + } + } + + /* Store the current value of the objective function. */ + pf[k % param.past] = fx; + } + + if (param.max_iterations != 0 && param.max_iterations < k + 1) + { + /* Maximum number of iterations. */ + ret = LBFGSERR_MAXIMUMITERATION; + break; + } + + /* + Update vectors s and y: + s_{k+1} = x_{k+1} - x_{k} = \step * d_{k}. + y_{k+1} = g_{k+1} - g_{k}. + */ + it = &lm[end]; + vecdiff(it->s, x, xp, n); + vecdiff(it->y, g, gp, n); + + /* + Compute scalars ys and yy: + ys = y^t \cdot s = 1 / \rho. + yy = y^t \cdot y. + Notice that yy is used for scaling the hessian matrix H_0 (Cholesky factor). + */ + vecdot(&ys, it->y, it->s, n); + vecdot(&yy, it->y, it->y, n); + it->ys = ys; + + /* + Recursive formula to compute dir = -(H \cdot g). + This is described in page 779 of: + Jorge Nocedal. + Updating Quasi-Newton Matrices with Limited Storage. + Mathematics of Computation, Vol. 35, No. 151, + pp. 773--782, 1980. + */ + bound = (m <= k) ? m : k; + ++k; + end = (end + 1) % m; + + /* Compute the negative of gradients. */ + vecncpy(d, g, n); + + j = end; + for (i = 0; i < bound; ++i) + { + j = (j + m - 1) % m; /* if (--j == -1) j = m-1; */ + it = &lm[j]; + /* \alpha_{j} = \rho_{j} s^{t}_{j} \cdot q_{k+1}. */ + vecdot(&it->alpha, it->s, d, n); + it->alpha /= it->ys; + /* q_{i} = q_{i+1} - \alpha_{i} y_{i}. */ + vecadd(d, it->y, -it->alpha, n); + } + + vecscale(d, ys / yy, n); + + for (i = 0; i < bound; ++i) + { + it = &lm[j]; + /* \beta_{j} = \rho_{j} y^t_{j} \cdot \gamm_{i}. */ + vecdot(&beta, it->y, d, n); + beta /= it->ys; + /* \gamm_{i+1} = \gamm_{i} + (\alpha_{j} - \beta_{j}) s_{j}. */ + vecadd(d, it->s, it->alpha - beta, n); + j = (j + 1) % m; /* if (++j == m) j = 0; */ + } + + /* + Now the search direction d is ready. We try step = 1 first. + */ + step = 1.0; + } + } + + /* Return the final value of the objective function. */ + if (ptr_fx != NULL) + { + *ptr_fx = fx; + } + + vecfree(pf); + + /* Free memory blocks used by this function. */ + if (lm != NULL) + { + for (i = 0; i < m; ++i) + { + vecfree(lm[i].s); + vecfree(lm[i].y); + } + vecfree(lm); + } + vecfree(d); + vecfree(gp); + vecfree(g); + vecfree(xp); + + return ret; + } + + /** + * Get string description of an lbfgs_optimize() return code. + * + * @param err A value returned by lbfgs_optimize(). + */ + inline const char *lbfgs_strerror(int err) + { + switch (err) + { + case LBFGS_CONVERGENCE: + return "Success: reached convergence (g_epsilon)."; + + case LBFGS_STOP: + return "Success: met stopping criteria (past f decrease less than delta)."; + + case LBFGS_ALREADY_MINIMIZED: + return "The initial variables already minimize the objective function."; + + case LBFGSERR_UNKNOWNERROR: + return "Unknown error."; + + case LBFGSERR_LOGICERROR: + return "Logic error."; + + case LBFGSERR_CANCELED: + return "The minimization process has been canceled."; + + case LBFGSERR_INVALID_N: + return "Invalid number of variables specified."; + + case LBFGSERR_INVALID_MEMSIZE: + return "Invalid parameter lbfgs_parameter_t::mem_size specified."; + + case LBFGSERR_INVALID_GEPSILON: + return "Invalid parameter lbfgs_parameter_t::g_epsilon specified."; + + case LBFGSERR_INVALID_TESTPERIOD: + return "Invalid parameter lbfgs_parameter_t::past specified."; + + case LBFGSERR_INVALID_DELTA: + return "Invalid parameter lbfgs_parameter_t::delta specified."; + + case LBFGSERR_INVALID_MINSTEP: + return "Invalid parameter lbfgs_parameter_t::min_step specified."; + + case LBFGSERR_INVALID_MAXSTEP: + return "Invalid parameter lbfgs_parameter_t::max_step specified."; + + case LBFGSERR_INVALID_FDECCOEFF: + return "Invalid parameter lbfgs_parameter_t::f_dec_coeff specified."; + + case LBFGSERR_INVALID_SCURVCOEFF: + return "Invalid parameter lbfgs_parameter_t::s_curv_coeff specified."; + + case LBFGSERR_INVALID_XTOL: + return "Invalid parameter lbfgs_parameter_t::xtol specified."; + + case LBFGSERR_INVALID_MAXLINESEARCH: + return "Invalid parameter lbfgs_parameter_t::max_linesearch specified."; + + case LBFGSERR_OUTOFINTERVAL: + return "The line-search step went out of the interval of uncertainty."; + + case LBFGSERR_INCORRECT_TMINMAX: + return "A logic error occurred; alternatively, the interval of uncertainty" + " became too small."; + + case LBFGSERR_ROUNDING_ERROR: + return "A rounding error occurred; alternatively, no line-search step" + " satisfies the sufficient decrease and curvature conditions."; + + case LBFGSERR_MINIMUMSTEP: + return "The line-search step became smaller than lbfgs_parameter_t::min_step."; + + case LBFGSERR_MAXIMUMSTEP: + return "The line-search step became larger than lbfgs_parameter_t::max_step."; + + case LBFGSERR_MAXIMUMLINESEARCH: + return "The line-search routine reaches the maximum number of evaluations."; + + case LBFGSERR_MAXIMUMITERATION: + return "The algorithm routine reaches the maximum number of iterations."; + + case LBFGSERR_WIDTHTOOSMALL: + return "Relative width of the interval of uncertainty is at most" + " lbfgs_parameter_t::xtol."; + + case LBFGSERR_INVALIDPARAMETERS: + return "A logic error (negative line-search step) occurred."; + + case LBFGSERR_INCREASEGRADIENT: + return "The current search direction increases the objective function value."; + + default: + return "(unknown)"; + } + } + +} // namespace lbfgs + +#endif \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/uniform_bspline.h b/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/uniform_bspline.h new file mode 100644 index 0000000..d2507a2 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/bspline_opt/include/bspline_opt/uniform_bspline.h @@ -0,0 +1,80 @@ +#ifndef _UNIFORM_BSPLINE_H_ +#define _UNIFORM_BSPLINE_H_ + +#include +#include +#include + +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 &point_set, + const vector &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 \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/bspline_opt/package.xml b/motion_planning/3d/ego_planner/planner/bspline_opt/package.xml new file mode 100755 index 0000000..02e098b --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/bspline_opt/package.xml @@ -0,0 +1,77 @@ + + + bspline_opt + 0.0.0 + The bspline_opt package + + + + + iszhouxin + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + plan_env + path_searching + traj_utils + roscpp + rospy + std_msgs + plan_env + path_searching + traj_utils + roscpp + rospy + std_msgs + plan_env + path_searching + traj_utils + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/bspline_opt/src/bspline_optimizer.cpp b/motion_planning/3d/ego_planner/planner/bspline_opt/src/bspline_optimizer.cpp new file mode 100644 index 0000000..49cca24 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/bspline_opt/src/bspline_optimizer.cpp @@ -0,0 +1,1730 @@ +#include "bspline_opt/bspline_optimizer.h" +#include "bspline_opt/gradient_descent_optimizer.h" +// using namespace std; + +namespace ego_planner +{ + + void BsplineOptimizer::setParam(ros::NodeHandle &nh) + { + nh.param("optimization/lambda_smooth", lambda1_, -1.0); + nh.param("optimization/lambda_collision", lambda2_, -1.0); + nh.param("optimization/lambda_feasibility", lambda3_, -1.0); + nh.param("optimization/lambda_fitness", lambda4_, -1.0); + + nh.param("optimization/dist0", dist0_, -1.0); + nh.param("optimization/swarm_clearance", swarm_clearance_, -1.0); + nh.param("optimization/max_vel", max_vel_, -1.0); + nh.param("optimization/max_acc", max_acc_, -1.0); + + nh.param("optimization/order", order_, 3); + } + + void BsplineOptimizer::setEnvironment(const GridMap::Ptr &map) + { + this->grid_map_ = map; + } + + void BsplineOptimizer::setEnvironment(const GridMap::Ptr &map, const fast_planner::ObjPredictor::Ptr mov_obj) + { + this->grid_map_ = map; + this->moving_objs_ = mov_obj; + } + + void BsplineOptimizer::setControlPoints(const Eigen::MatrixXd &points) + { + cps_.points = points; + } + + void BsplineOptimizer::setBsplineInterval(const double &ts) { bspline_interval_ = ts; } + + void BsplineOptimizer::setSwarmTrajs(SwarmTrajData *swarm_trajs_ptr) { swarm_trajs_ = swarm_trajs_ptr; } + + void BsplineOptimizer::setDroneId(const int drone_id) { drone_id_ = drone_id; } + + std::vector BsplineOptimizer::distinctiveTrajs(vector> segments) + { + if (segments.size() == 0) // will be invoked again later. + { + std::vector oneSeg; + oneSeg.push_back(cps_); + return oneSeg; + } + + // cout << "A1" << endl; + + constexpr int MAX_TRAJS = 8; + constexpr int VARIS = 2; + int seg_upbound = std::min((int)segments.size(), static_cast(floor(log(MAX_TRAJS) / log(VARIS)))); + std::vector control_pts_buf; + control_pts_buf.reserve(MAX_TRAJS); + const double RESOLUTION = grid_map_->getResolution(); + const double CTRL_PT_DIST = (cps_.points.col(0) - cps_.points.col(cps_.size - 1)).norm() / (cps_.size - 1); + + // Step 1. Find the opposite vectors and base points for every segment. + std::vector> RichInfoSegs; + for (int i = 0; i < seg_upbound; i++) + { + std::pair RichInfoOneSeg; + ControlPoints RichInfoOneSeg_temp; + cps_.segment(RichInfoOneSeg_temp, segments[i].first, segments[i].second); + RichInfoOneSeg.first = RichInfoOneSeg_temp; + RichInfoOneSeg.second = RichInfoOneSeg_temp; + RichInfoSegs.push_back(RichInfoOneSeg); + + // cout << "RichInfoOneSeg_temp, out" << endl; + // cout << "RichInfoSegs[" << i << "].first" << endl; + // for ( int k=0; k 0 ) + // { + // cout << "###" << RichInfoOneSeg_temp.points.col(k).transpose() << endl; + // for (int k2 = 0; k2 < RichInfoOneSeg_temp.base_point[k].size(); k2++) + // { + // cout << " " << RichInfoOneSeg_temp.base_point[k][k2].transpose() << " @ " << RichInfoOneSeg_temp.direction[k][k2].transpose() << endl; + // } + // } + } + + // cout << "A2" << endl; + + for (int i = 0; i < seg_upbound; i++) + { + + // 1.1 Find the start occupied point id and the last occupied point id + if (RichInfoSegs[i].first.size > 1) + { + int occ_start_id = -1, occ_end_id = -1; + Eigen::Vector3d occ_start_pt, occ_end_pt; + for (int j = 0; j < RichInfoSegs[i].first.size - 1; j++) + { + //cout << "A *" << j << "*" << endl; + double step_size = RESOLUTION / (RichInfoSegs[i].first.points.col(j) - RichInfoSegs[i].first.points.col(j + 1)).norm() / 2; + for (double a = 1; a > 0; a -= step_size) + { + Eigen::Vector3d pt(a * RichInfoSegs[i].first.points.col(j) + (1 - a) * RichInfoSegs[i].first.points.col(j + 1)); + //cout << " " << grid_map_->getInflateOccupancy(pt) << " pt=" << pt.transpose() << endl; + if (grid_map_->getInflateOccupancy(pt)) + { + occ_start_id = j; + occ_start_pt = pt; + goto exit_multi_loop1; + } + } + } + exit_multi_loop1:; + for (int j = RichInfoSegs[i].first.size - 1; j >= 1; j--) + { + //cout << "j=" << j << endl; + //cout << "B *" << j << "*" << endl; + ; + double step_size = RESOLUTION / (RichInfoSegs[i].first.points.col(j) - RichInfoSegs[i].first.points.col(j - 1)).norm(); + for (double a = 1; a > 0; a -= step_size) + { + Eigen::Vector3d pt(a * RichInfoSegs[i].first.points.col(j) + (1 - a) * RichInfoSegs[i].first.points.col(j - 1)); + //cout << " " << grid_map_->getInflateOccupancy(pt) << " pt=" << pt.transpose() << endl; + ; + if (grid_map_->getInflateOccupancy(pt)) + { + occ_end_id = j; + occ_end_pt = pt; + goto exit_multi_loop2; + } + } + } + exit_multi_loop2:; + + // double check + if (occ_start_id == -1 || occ_end_id == -1) + { + // It means that the first or the last control points of one segment are in obstacles, which is not allowed. + // ROS_WARN("What? occ_start_id=%d, occ_end_id=%d", occ_start_id, occ_end_id); + + segments.erase(segments.begin() + i); + RichInfoSegs.erase(RichInfoSegs.begin() + i); + seg_upbound--; + i--; + + continue; + + // cout << "RichInfoSegs[" << i << "].first" << endl; + // for (int k = 0; k < RichInfoSegs[i].first.size; k++) + // { + // if (RichInfoSegs[i].first.base_point.size() > 0) + // { + // cout << "###" << RichInfoSegs[i].first.points.col(k).transpose() << endl; + // for (int k2 = 0; k2 < RichInfoSegs[i].first.base_point[k].size(); k2++) + // { + // cout << " " << RichInfoSegs[i].first.base_point[k][k2].transpose() << " @ " << RichInfoSegs[i].first.direction[k][k2].transpose() << endl; + // } + // } + // } + } + + // 1.2 Reverse the vector and find new base points from occ_start_id to occ_end_id. + for (int j = occ_start_id; j <= occ_end_id; j++) + { + Eigen::Vector3d base_pt_reverse, base_vec_reverse; + if (RichInfoSegs[i].first.base_point[j].size() != 1) + { + cout << "RichInfoSegs[" << i << "].first.base_point[" << j << "].size()=" << RichInfoSegs[i].first.base_point[j].size() << endl; + ROS_ERROR("Wrong number of base_points!!! Should not be happen!."); + + cout << setprecision(5); + cout << "cps_" << endl; + cout << " clearance=" << cps_.clearance << " cps.size=" << cps_.size << endl; + for (int temp_i = 0; temp_i < cps_.size; temp_i++) + { + if (cps_.base_point[temp_i].size() > 1 && cps_.base_point[temp_i].size() < 1000) + { + ROS_ERROR("Should not happen!!!"); + cout << "######" << cps_.points.col(temp_i).transpose() << endl; + for (size_t temp_j = 0; temp_j < cps_.base_point[temp_i].size(); temp_j++) + cout << " " << cps_.base_point[temp_i][temp_j].transpose() << " @ " << cps_.direction[temp_i][temp_j].transpose() << endl; + } + } + + std::vector blank; + return blank; + } + + base_vec_reverse = -RichInfoSegs[i].first.direction[j][0]; + + // The start and the end case must get taken special care of. + if (j == occ_start_id) + { + base_pt_reverse = occ_start_pt; + } + else if (j == occ_end_id) + { + base_pt_reverse = occ_end_pt; + } + else + { + base_pt_reverse = RichInfoSegs[i].first.points.col(j) + base_vec_reverse * (RichInfoSegs[i].first.base_point[j][0] - RichInfoSegs[i].first.points.col(j)).norm(); + } + + if (grid_map_->getInflateOccupancy(base_pt_reverse)) // Search outward. + { + double l_upbound = 5 * CTRL_PT_DIST; // "5" is the threshold. + double l = RESOLUTION; + for (; l <= l_upbound; l += RESOLUTION) + { + Eigen::Vector3d base_pt_temp = base_pt_reverse + l * base_vec_reverse; + //cout << base_pt_temp.transpose() << endl; + if (!grid_map_->getInflateOccupancy(base_pt_temp)) + { + RichInfoSegs[i].second.base_point[j][0] = base_pt_temp; + RichInfoSegs[i].second.direction[j][0] = base_vec_reverse; + break; + } + } + if (l > l_upbound) + { + ROS_WARN("Can't find the new base points at the opposite within the threshold. i=%d, j=%d", i, j); + + segments.erase(segments.begin() + i); + RichInfoSegs.erase(RichInfoSegs.begin() + i); + seg_upbound--; + i--; + + goto exit_multi_loop3; // break "for (int j = 0; j < RichInfoSegs[i].first.size; j++)" + } + } + else if ((base_pt_reverse - RichInfoSegs[i].first.points.col(j)).norm() >= RESOLUTION) // Unnecessary to search. + { + RichInfoSegs[i].second.base_point[j][0] = base_pt_reverse; + RichInfoSegs[i].second.direction[j][0] = base_vec_reverse; + } + else + { + ROS_WARN("base_point and control point are too close!"); + cout << "base_point=" << RichInfoSegs[i].first.base_point[j][0].transpose() << " control point=" << RichInfoSegs[i].first.points.col(j).transpose() << endl; + + segments.erase(segments.begin() + i); + RichInfoSegs.erase(RichInfoSegs.begin() + i); + seg_upbound--; + i--; + + goto exit_multi_loop3; // break "for (int j = 0; j < RichInfoSegs[i].first.size; j++)" + } + } + + // 1.3 Assign the base points to control points within [0, occ_start_id) and (occ_end_id, RichInfoSegs[i].first.size()-1]. + if (RichInfoSegs[i].second.size) + { + for (int j = occ_start_id - 1; j >= 0; j--) + { + RichInfoSegs[i].second.base_point[j][0] = RichInfoSegs[i].second.base_point[occ_start_id][0]; + RichInfoSegs[i].second.direction[j][0] = RichInfoSegs[i].second.direction[occ_start_id][0]; + } + for (int j = occ_end_id + 1; j < RichInfoSegs[i].second.size; j++) + { + RichInfoSegs[i].second.base_point[j][0] = RichInfoSegs[i].second.base_point[occ_end_id][0]; + RichInfoSegs[i].second.direction[j][0] = RichInfoSegs[i].second.direction[occ_end_id][0]; + } + } + + exit_multi_loop3:; + } + else + { + Eigen::Vector3d base_vec_reverse = -RichInfoSegs[i].first.direction[0][0]; + Eigen::Vector3d base_pt_reverse = RichInfoSegs[i].first.points.col(0) + base_vec_reverse * (RichInfoSegs[i].first.base_point[0][0] - RichInfoSegs[i].first.points.col(0)).norm(); + + if (grid_map_->getInflateOccupancy(base_pt_reverse)) // Search outward. + { + double l_upbound = 5 * CTRL_PT_DIST; // "5" is the threshold. + double l = RESOLUTION; + for (; l <= l_upbound; l += RESOLUTION) + { + Eigen::Vector3d base_pt_temp = base_pt_reverse + l * base_vec_reverse; + //cout << base_pt_temp.transpose() << endl; + if (!grid_map_->getInflateOccupancy(base_pt_temp)) + { + RichInfoSegs[i].second.base_point[0][0] = base_pt_temp; + RichInfoSegs[i].second.direction[0][0] = base_vec_reverse; + break; + } + } + if (l > l_upbound) + { + ROS_WARN("Can't find the new base points at the opposite within the threshold, 2. i=%d", i); + + segments.erase(segments.begin() + i); + RichInfoSegs.erase(RichInfoSegs.begin() + i); + seg_upbound--; + i--; + } + } + else if ((base_pt_reverse - RichInfoSegs[i].first.points.col(0)).norm() >= RESOLUTION) // Unnecessary to search. + { + RichInfoSegs[i].second.base_point[0][0] = base_pt_reverse; + RichInfoSegs[i].second.direction[0][0] = base_vec_reverse; + } + else + { + ROS_WARN("base_point and control point are too close!, 2"); + cout << "base_point=" << RichInfoSegs[i].first.base_point[0][0].transpose() << " control point=" << RichInfoSegs[i].first.points.col(0).transpose() << endl; + + segments.erase(segments.begin() + i); + RichInfoSegs.erase(RichInfoSegs.begin() + i); + seg_upbound--; + i--; + } + } + + // cout << "A3" << endl; + } + + // Step 2. Assemble each segment to make up the new control point sequence. + if (seg_upbound == 0) // After the erase operation above, segment legth will decrease to 0 again. + { + std::vector oneSeg; + oneSeg.push_back(cps_); + return oneSeg; + } + + // cout << "A4" << endl; + + std::vector selection(seg_upbound); + std::fill(selection.begin(), selection.end(), 0); + selection[0] = -1; // init + int max_traj_nums = static_cast(pow(VARIS, seg_upbound)); + for (int i = 0; i < max_traj_nums; i++) + { + // 2.1 Calculate the selection table. + int digit_id = 0; + selection[digit_id]++; + while (digit_id < seg_upbound && selection[digit_id] >= VARIS) + { + selection[digit_id] = 0; + digit_id++; + if (digit_id >= seg_upbound) + { + ROS_ERROR("Should not happen!!! digit_id=%d, seg_upbound=%d", digit_id, seg_upbound); + } + selection[digit_id]++; + } + + // 2.2 Assign params according to the selection table. + ControlPoints cpsOneSample; + cpsOneSample.resize(cps_.size); + cpsOneSample.clearance = cps_.clearance; + int cp_id = 0, seg_id = 0, cp_of_seg_id = 0; + while (/*seg_id < RichInfoSegs.size() ||*/ cp_id < cps_.size) + { + //cout << "A "; + // if ( seg_id >= RichInfoSegs.size() ) + // { + // cout << "seg_id=" << seg_id << " RichInfoSegs.size()=" << RichInfoSegs.size() << endl; + // } + // if ( cp_id >= cps_.base_point.size() ) + // { + // cout << "cp_id=" << cp_id << " cps_.base_point.size()=" << cps_.base_point.size() << endl; + // } + // if ( cp_of_seg_id >= RichInfoSegs[seg_id].first.base_point.size() ) + // { + // cout << "cp_of_seg_id=" << cp_of_seg_id << " RichInfoSegs[seg_id].first.base_point.size()=" << RichInfoSegs[seg_id].first.base_point.size() << endl; + // } + + if (seg_id >= seg_upbound || cp_id < segments[seg_id].first || cp_id > segments[seg_id].second) + { + cpsOneSample.points.col(cp_id) = cps_.points.col(cp_id); + cpsOneSample.base_point[cp_id] = cps_.base_point[cp_id]; + cpsOneSample.direction[cp_id] = cps_.direction[cp_id]; + } + else if (cp_id >= segments[seg_id].first && cp_id <= segments[seg_id].second) + { + if (!selection[seg_id]) // zx-todo + { + cpsOneSample.points.col(cp_id) = RichInfoSegs[seg_id].first.points.col(cp_of_seg_id); + cpsOneSample.base_point[cp_id] = RichInfoSegs[seg_id].first.base_point[cp_of_seg_id]; + cpsOneSample.direction[cp_id] = RichInfoSegs[seg_id].first.direction[cp_of_seg_id]; + cp_of_seg_id++; + } + else + { + if (RichInfoSegs[seg_id].second.size) + { + cpsOneSample.points.col(cp_id) = RichInfoSegs[seg_id].second.points.col(cp_of_seg_id); + cpsOneSample.base_point[cp_id] = RichInfoSegs[seg_id].second.base_point[cp_of_seg_id]; + cpsOneSample.direction[cp_id] = RichInfoSegs[seg_id].second.direction[cp_of_seg_id]; + cp_of_seg_id++; + } + else + { + // Abandon this trajectory. + goto abandon_this_trajectory; + } + } + + if (cp_id == segments[seg_id].second) + { + cp_of_seg_id = 0; + seg_id++; + } + } + else + { + ROS_ERROR("Shold not happen!!!!, cp_id=%d, seg_id=%d, segments.front().first=%d, segments.back().second=%d, segments[seg_id].first=%d, segments[seg_id].second=%d", + cp_id, seg_id, segments.front().first, segments.back().second, segments[seg_id].first, segments[seg_id].second); + } + + cp_id++; + } + + control_pts_buf.push_back(cpsOneSample); + + abandon_this_trajectory:; + } + + // cout << "A5" << endl; + + return control_pts_buf; + } // namespace ego_planner + + /* This function is very similar to check_collision_and_rebound(). + * It was written separately, just because I did it once and it has been running stably since March 2020. + * But I will merge then someday.*/ + std::vector> BsplineOptimizer::initControlPoints(Eigen::MatrixXd &init_points, bool flag_first_init /*= true*/) + { + + if (flag_first_init) + { + cps_.clearance = dist0_; + cps_.resize(init_points.cols()); + cps_.points = init_points; + } + + /*** Segment the initial trajectory according to obstacles ***/ + constexpr int ENOUGH_INTERVAL = 2; + double step_size = grid_map_->getResolution() / ((init_points.col(0) - init_points.rightCols(1)).norm() / (init_points.cols() - 1)) / 1.5; + int in_id, out_id; + vector> segment_ids; + int same_occ_state_times = ENOUGH_INTERVAL + 1; + bool occ, last_occ = false; + bool flag_got_start = false, flag_got_end = false, flag_got_end_maybe = false; + int i_end = (int)init_points.cols() - order_ - ((int)init_points.cols() - 2 * order_) / 3; // only check closed 2/3 points. + for (int i = order_; i <= i_end; ++i) + { + //cout << " *" << i-1 << "*" ; + for (double a = 1.0; a > 0.0; a -= step_size) + { + occ = grid_map_->getInflateOccupancy(a * init_points.col(i - 1) + (1 - a) * init_points.col(i)); + //cout << " " << occ; + // cout << setprecision(5); + // cout << (a * init_points.col(i-1) + (1-a) * init_points.col(i)).transpose() << " occ1=" << occ << endl; + + if (occ && !last_occ) + { + if (same_occ_state_times > ENOUGH_INTERVAL || i == order_) + { + in_id = i - 1; + flag_got_start = true; + } + same_occ_state_times = 0; + flag_got_end_maybe = false; // terminate in advance + } + else if (!occ && last_occ) + { + out_id = i; + flag_got_end_maybe = true; + same_occ_state_times = 0; + } + else + { + ++same_occ_state_times; + } + + if (flag_got_end_maybe && (same_occ_state_times > ENOUGH_INTERVAL || (i == (int)init_points.cols() - order_))) + { + flag_got_end_maybe = false; + flag_got_end = true; + } + + last_occ = occ; + + if (flag_got_start && flag_got_end) + { + flag_got_start = false; + flag_got_end = false; + segment_ids.push_back(std::pair(in_id, out_id)); + } + } + } + // cout << endl; + + // for (size_t i = 0; i < segment_ids.size(); i++) + // { + // cout << "segment_ids=" << segment_ids[i].first << " ~ " << segment_ids[i].second << endl; + // } + + // return in advance + if (segment_ids.size() == 0) + { + vector> blank_ret; + return blank_ret; + } + + /*** a star search ***/ + vector> a_star_pathes; + for (size_t i = 0; i < segment_ids.size(); ++i) + { + //cout << "in=" << in.transpose() << " out=" << out.transpose() << endl; + Eigen::Vector3d in(init_points.col(segment_ids[i].first)), out(init_points.col(segment_ids[i].second)); + if (a_star_->AstarSearch(/*(in-out).norm()/10+0.05*/ 0.1, in, out)) + { + a_star_pathes.push_back(a_star_->getPath()); + } + else + { + ROS_ERROR("a star error, force return!"); + vector> blank_ret; + return blank_ret; + } + } + + /*** calculate bounds ***/ + int id_low_bound, id_up_bound; + vector> bounds(segment_ids.size()); + for (size_t i = 0; i < segment_ids.size(); i++) + { + + if (i == 0) // first segment + { + id_low_bound = order_; + if (segment_ids.size() > 1) + { + id_up_bound = (int)(((segment_ids[0].second + segment_ids[1].first) - 1.0f) / 2); // id_up_bound : -1.0f fix() + } + else + { + id_up_bound = init_points.cols() - order_ - 1; + } + } + else if (i == segment_ids.size() - 1) // last segment, i != 0 here + { + id_low_bound = (int)(((segment_ids[i].first + segment_ids[i - 1].second) + 1.0f) / 2); // id_low_bound : +1.0f ceil() + id_up_bound = init_points.cols() - order_ - 1; + } + else + { + id_low_bound = (int)(((segment_ids[i].first + segment_ids[i - 1].second) + 1.0f) / 2); // id_low_bound : +1.0f ceil() + id_up_bound = (int)(((segment_ids[i].second + segment_ids[i + 1].first) - 1.0f) / 2); // id_up_bound : -1.0f fix() + } + + bounds[i] = std::pair(id_low_bound, id_up_bound); + } + + // cout << "+++++++++" << endl; + // for ( int j=0; j> adjusted_segment_ids(segment_ids.size()); + constexpr double MINIMUM_PERCENT = 0.0; // Each segment is guaranteed to have sufficient points to generate sufficient force + int minimum_points = round(init_points.cols() * MINIMUM_PERCENT), num_points; + for (size_t i = 0; i < segment_ids.size(); i++) + { + /*** Adjust segment length ***/ + num_points = segment_ids[i].second - segment_ids[i].first + 1; + //cout << "i = " << i << " first = " << segment_ids[i].first << " second = " << segment_ids[i].second << endl; + if (num_points < minimum_points) + { + double add_points_each_side = (int)(((minimum_points - num_points) + 1.0f) / 2); + + adjusted_segment_ids[i].first = segment_ids[i].first - add_points_each_side >= bounds[i].first ? segment_ids[i].first - add_points_each_side : bounds[i].first; + + adjusted_segment_ids[i].second = segment_ids[i].second + add_points_each_side <= bounds[i].second ? segment_ids[i].second + add_points_each_side : bounds[i].second; + } + else + { + adjusted_segment_ids[i].first = segment_ids[i].first; + adjusted_segment_ids[i].second = segment_ids[i].second; + } + + //cout << "final:" << "i = " << i << " first = " << adjusted_segment_ids[i].first << " second = " << adjusted_segment_ids[i].second << endl; + } + for (size_t i = 1; i < adjusted_segment_ids.size(); i++) // Avoid overlap + { + if (adjusted_segment_ids[i - 1].second >= adjusted_segment_ids[i].first) + { + double middle = (double)(adjusted_segment_ids[i - 1].second + adjusted_segment_ids[i].first) / 2.0; + adjusted_segment_ids[i - 1].second = static_cast(middle - 0.1); + adjusted_segment_ids[i].first = static_cast(middle + 1.1); + } + } + + // Used for return + vector> final_segment_ids; + + /*** Assign data to each segment ***/ + for (size_t i = 0; i < segment_ids.size(); i++) + { + // step 1 + for (int j = adjusted_segment_ids[i].first; j <= adjusted_segment_ids[i].second; ++j) + cps_.flag_temp[j] = false; + + // step 2 + int got_intersection_id = -1; + for (int j = segment_ids[i].first + 1; j < segment_ids[i].second; ++j) + { + Eigen::Vector3d ctrl_pts_law(init_points.col(j + 1) - init_points.col(j - 1)), intersection_point; + int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation + double val = (a_star_pathes[i][Astar_id] - init_points.col(j)).dot(ctrl_pts_law), last_val = val; + while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()) + { + last_Astar_id = Astar_id; + + if (val >= 0) + --Astar_id; + else + ++Astar_id; + + val = (a_star_pathes[i][Astar_id] - init_points.col(j)).dot(ctrl_pts_law); + + if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed + { + intersection_point = + a_star_pathes[i][Astar_id] + + ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) * + (ctrl_pts_law.dot(init_points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t + ); + + //cout << "i=" << i << " j=" << j << " Astar_id=" << Astar_id << " last_Astar_id=" << last_Astar_id << " intersection_point = " << intersection_point.transpose() << endl; + + got_intersection_id = j; + break; + } + } + + if (got_intersection_id >= 0) + { + double length = (intersection_point - init_points.col(j)).norm(); + if (length > 1e-5) + { + cps_.flag_temp[j] = true; + for (double a = length; a >= 0.0; a -= grid_map_->getResolution()) + { + occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * init_points.col(j)); + + if (occ || a < grid_map_->getResolution()) + { + if (occ) + a += grid_map_->getResolution(); + cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * init_points.col(j)); + cps_.direction[j].push_back((intersection_point - init_points.col(j)).normalized()); + // cout << "A " << j << endl; + break; + } + } + } + else + { + got_intersection_id = -1; + } + } + } + + /* Corner case: the segment length is too short. Here the control points may outside the A* path, leading to opposite gradient direction. So I have to take special care of it */ + if (segment_ids[i].second - segment_ids[i].first == 1) + { + Eigen::Vector3d ctrl_pts_law(init_points.col(segment_ids[i].second) - init_points.col(segment_ids[i].first)), intersection_point; + Eigen::Vector3d middle_point = (init_points.col(segment_ids[i].second) + init_points.col(segment_ids[i].first)) / 2; + int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation + double val = (a_star_pathes[i][Astar_id] - middle_point).dot(ctrl_pts_law), last_val = val; + while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()) + { + last_Astar_id = Astar_id; + + if (val >= 0) + --Astar_id; + else + ++Astar_id; + + val = (a_star_pathes[i][Astar_id] - middle_point).dot(ctrl_pts_law); + + if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed + { + intersection_point = + a_star_pathes[i][Astar_id] + + ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) * + (ctrl_pts_law.dot(middle_point - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t + ); + + if ((intersection_point - middle_point).norm() > 0.01) // 1cm. + { + cps_.flag_temp[segment_ids[i].first] = true; + cps_.base_point[segment_ids[i].first].push_back(init_points.col(segment_ids[i].first)); + cps_.direction[segment_ids[i].first].push_back((intersection_point - middle_point).normalized()); + // cout << "AA " << segment_ids[i].first << endl; + + got_intersection_id = segment_ids[i].first; + } + break; + } + } + } + + //step 3 + if (got_intersection_id >= 0) + { + for (int j = got_intersection_id + 1; j <= adjusted_segment_ids[i].second; ++j) + if (!cps_.flag_temp[j]) + { + cps_.base_point[j].push_back(cps_.base_point[j - 1].back()); + cps_.direction[j].push_back(cps_.direction[j - 1].back()); + // cout << "AAA " << j << endl; + } + + for (int j = got_intersection_id - 1; j >= adjusted_segment_ids[i].first; --j) + if (!cps_.flag_temp[j]) + { + cps_.base_point[j].push_back(cps_.base_point[j + 1].back()); + cps_.direction[j].push_back(cps_.direction[j + 1].back()); + // cout << "AAAA " << j << endl; + } + + final_segment_ids.push_back(adjusted_segment_ids[i]); + } + else + { + // Just ignore, it does not matter ^_^. + // ROS_ERROR("Failed to generate direction! segment_id=%d", i); + } + } + + return final_segment_ids; + } + + int BsplineOptimizer::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) + { + BsplineOptimizer *opt = reinterpret_cast(func_data); + // cout << "k=" << k << endl; + // cout << "opt->flag_continue_to_optimize_=" << opt->flag_continue_to_optimize_ << endl; + return (opt->force_stop_type_ == STOP_FOR_ERROR || opt->force_stop_type_ == STOP_FOR_REBOUND); + } + + double BsplineOptimizer::costFunctionRebound(void *func_data, const double *x, double *grad, const int n) + { + BsplineOptimizer *opt = reinterpret_cast(func_data); + + double cost; + opt->combineCostRebound(x, grad, cost, n); + + opt->iter_num_ += 1; + return cost; + } + + double BsplineOptimizer::costFunctionRefine(void *func_data, const double *x, double *grad, const int n) + { + BsplineOptimizer *opt = reinterpret_cast(func_data); + + double cost; + opt->combineCostRefine(x, grad, cost, n); + + opt->iter_num_ += 1; + return cost; + } + + void BsplineOptimizer::calcSwarmCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) + { + cost = 0.0; + int end_idx = q.cols() - order_ - (double)(q.cols() - 2*order_)*1.0/3.0; // Only check the first 2/3 points + const double CLEARANCE = swarm_clearance_ * 2; + double t_now = ros::Time::now().toSec(); + constexpr double a = 2.0, b = 1.0, inv_a2 = 1 / a / a, inv_b2 = 1 / b / b; + + for (int i = order_; i < end_idx; i++) + { + double glb_time = t_now + ((double)(order_ - 1) / 2 + (i - order_ + 1)) * bspline_interval_; + + for (size_t id = 0; id < swarm_trajs_->size(); id++) + { + if ( (swarm_trajs_->at(id).drone_id != (int)id) || swarm_trajs_->at(id).drone_id == drone_id_ ) + { + continue; + } + + double traj_i_satrt_time = swarm_trajs_->at(id).start_time_.toSec(); + if (glb_time < traj_i_satrt_time + swarm_trajs_->at(id).duration_ - 0.1) + { + /* def cost=(c-sqrt([Q-O]'D[Q-O]))^2, D=[1/b^2,0,0;0,1/b^2,0;0,0,1/a^2] */ + Eigen::Vector3d swarm_prid = swarm_trajs_->at(id).position_traj_.evaluateDeBoorT(glb_time - traj_i_satrt_time); + Eigen::Vector3d dist_vec = cps_.points.col(i) - swarm_prid; + double ellip_dist = sqrt(dist_vec(2) * dist_vec(2) * inv_a2 + (dist_vec(0) * dist_vec(0) + dist_vec(1) * dist_vec(1)) * inv_b2); + double dist_err = CLEARANCE - ellip_dist; + + Eigen::Vector3d dist_grad = cps_.points.col(i) - swarm_prid; + Eigen::Vector3d Coeff; + Coeff(0) = -2 * (CLEARANCE / ellip_dist - 1) * inv_b2; + Coeff(1) = Coeff(0); + Coeff(2) = -2 * (CLEARANCE / ellip_dist - 1) * inv_a2; + + if (dist_err < 0) + { + /* do nothing */ + } + else + { + cost += pow(dist_err, 2); + gradient.col(i) += (Coeff.array() * dist_grad.array()).matrix(); + } + + if (min_ellip_dist_ > dist_err) + { + min_ellip_dist_ = dist_err; + } + } + } + } + } + + void BsplineOptimizer::calcMovingObjCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) + { + cost = 0.0; + int end_idx = q.cols() - order_; + constexpr double CLEARANCE = 1.5; + double t_now = ros::Time::now().toSec(); + + for (int i = order_; i < end_idx; i++) + { + double time = ((double)(order_ - 1) / 2 + (i - order_ + 1)) * bspline_interval_; + + for (int id = 0; id < moving_objs_->getObjNums(); id++) + { + Eigen::Vector3d obj_prid = moving_objs_->evaluateConstVel(id, t_now + time); + double dist = (cps_.points.col(i) - obj_prid).norm(); + //cout /*<< "cps_.points.col(i)=" << cps_.points.col(i).transpose()*/ << " moving_objs_=" << obj_prid.transpose() << " dist=" << dist << endl; + double dist_err = CLEARANCE - dist; + Eigen::Vector3d dist_grad = (cps_.points.col(i) - obj_prid).normalized(); + + if (dist_err < 0) + { + /* do nothing */ + } + else + { + cost += pow(dist_err, 2); + gradient.col(i) += -2.0 * dist_err * dist_grad; + } + } + // cout << "time=" << time << " i=" << i << " order_=" << order_ << " end_idx=" << end_idx << endl; + // cout << "--" << endl; + } + // cout << "---------------" << endl; + } + + void BsplineOptimizer::calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost, + Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost) + { + cost = 0.0; + int end_idx = q.cols() - order_; + double demarcation = cps_.clearance; + double a = 3 * demarcation, b = -3 * pow(demarcation, 2), c = pow(demarcation, 3); + + force_stop_type_ = DONT_STOP; + if (iter_num > 3 && smoothness_cost / (cps_.size - 2 * order_) < 0.1) // 0.1 is an experimental value that indicates the trajectory is smooth enough. + { + check_collision_and_rebound(); + } + + /*** calculate distance cost and gradient ***/ + for (auto i = order_; i < end_idx; ++i) + { + for (size_t j = 0; j < cps_.direction[i].size(); ++j) + { + double dist = (cps_.points.col(i) - cps_.base_point[i][j]).dot(cps_.direction[i][j]); + double dist_err = cps_.clearance - dist; + Eigen::Vector3d dist_grad = cps_.direction[i][j]; + + if (dist_err < 0) + { + /* do nothing */ + } + else if (dist_err < demarcation) + { + cost += pow(dist_err, 3); + gradient.col(i) += -3.0 * dist_err * dist_err * dist_grad; + } + else + { + cost += a * dist_err * dist_err + b * dist_err + c; + gradient.col(i) += -(2.0 * a * dist_err + b) * dist_grad; + } + } + } + } + + void BsplineOptimizer::calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) + { + + cost = 0.0; + + int end_idx = q.cols() - order_; + + // def: f = |x*v|^2/a^2 + |x×v|^2/b^2 + double a2 = 25, b2 = 1; + for (auto i = order_ - 1; i < end_idx + 1; ++i) + { + Eigen::Vector3d x = (q.col(i - 1) + 4 * q.col(i) + q.col(i + 1)) / 6.0 - ref_pts_[i - 1]; + Eigen::Vector3d v = (ref_pts_[i] - ref_pts_[i - 2]).normalized(); + + double xdotv = x.dot(v); + Eigen::Vector3d xcrossv = x.cross(v); + + double f = pow((xdotv), 2) / a2 + pow(xcrossv.norm(), 2) / b2; + cost += f; + + Eigen::Matrix3d m; + m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0; + Eigen::Vector3d df_dx = 2 * xdotv / a2 * v + 2 / b2 * m * xcrossv; + + gradient.col(i - 1) += df_dx / 6; + gradient.col(i) += 4 * df_dx / 6; + gradient.col(i + 1) += df_dx / 6; + } + } + + void BsplineOptimizer::calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost, + Eigen::MatrixXd &gradient, bool falg_use_jerk /* = true*/) + { + + cost = 0.0; + + if (falg_use_jerk) + { + Eigen::Vector3d jerk, temp_j; + + for (int i = 0; i < q.cols() - 3; i++) + { + /* evaluate jerk */ + jerk = q.col(i + 3) - 3 * q.col(i + 2) + 3 * q.col(i + 1) - q.col(i); + cost += jerk.squaredNorm(); + temp_j = 2.0 * jerk; + /* jerk gradient */ + gradient.col(i + 0) += -temp_j; + gradient.col(i + 1) += 3.0 * temp_j; + gradient.col(i + 2) += -3.0 * temp_j; + gradient.col(i + 3) += temp_j; + } + } + else + { + Eigen::Vector3d acc, temp_acc; + + for (int i = 0; i < q.cols() - 2; i++) + { + /* evaluate acc */ + acc = q.col(i + 2) - 2 * q.col(i + 1) + q.col(i); + cost += acc.squaredNorm(); + temp_acc = 2.0 * acc; + /* acc gradient */ + gradient.col(i + 0) += temp_acc; + gradient.col(i + 1) += -2.0 * temp_acc; + gradient.col(i + 2) += temp_acc; + } + } + } + + void BsplineOptimizer::calcTerminalCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient) + { + cost = 0.0; + + // zero cost and gradient in hard constraints + Eigen::Vector3d q_3, q_2, q_1, dq; + q_3 = q.col(q.cols()-3); + q_2 = q.col(q.cols()-2); + q_1 = q.col(q.cols()-1); + + dq = 1 / 6.0 * (q_3 + 4 * q_2 + q_1) - local_target_pt_; + cost += dq.squaredNorm(); + + gradient.col(q.cols()-3) += 2 * dq * (1 / 6.0); + gradient.col(q.cols()-2) += 2 * dq * (4 / 6.0); + gradient.col(q.cols()-1) += 2 * dq * (1 / 6.0); + + } + + void BsplineOptimizer::calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost, + Eigen::MatrixXd &gradient) + { + + //#define SECOND_DERIVATIVE_CONTINOUS + +#ifdef SECOND_DERIVATIVE_CONTINOUS + + cost = 0.0; + double demarcation = 1.0; // 1m/s, 1m/s/s + double ar = 3 * demarcation, br = -3 * pow(demarcation, 2), cr = pow(demarcation, 3); + double al = ar, bl = -br, cl = cr; + + /* abbreviation */ + double ts, ts_inv2, ts_inv3; + ts = bspline_interval_; + ts_inv2 = 1 / ts / ts; + ts_inv3 = 1 / ts / ts / ts; + + /* velocity feasibility */ + for (int i = 0; i < q.cols() - 1; i++) + { + Eigen::Vector3d vi = (q.col(i + 1) - q.col(i)) / ts; + + for (int j = 0; j < 3; j++) + { + if (vi(j) > max_vel_ + demarcation) + { + double diff = vi(j) - max_vel_; + cost += (ar * diff * diff + br * diff + cr) * ts_inv3; // multiply ts_inv3 to make vel and acc has similar magnitude + + double grad = (2.0 * ar * diff + br) / ts * ts_inv3; + gradient(j, i + 0) += -grad; + gradient(j, i + 1) += grad; + } + else if (vi(j) > max_vel_) + { + double diff = vi(j) - max_vel_; + cost += pow(diff, 3) * ts_inv3; + ; + + double grad = 3 * diff * diff / ts * ts_inv3; + ; + gradient(j, i + 0) += -grad; + gradient(j, i + 1) += grad; + } + else if (vi(j) < -(max_vel_ + demarcation)) + { + double diff = vi(j) + max_vel_; + cost += (al * diff * diff + bl * diff + cl) * ts_inv3; + + double grad = (2.0 * al * diff + bl) / ts * ts_inv3; + gradient(j, i + 0) += -grad; + gradient(j, i + 1) += grad; + } + else if (vi(j) < -max_vel_) + { + double diff = vi(j) + max_vel_; + cost += -pow(diff, 3) * ts_inv3; + + double grad = -3 * diff * diff / ts * ts_inv3; + gradient(j, i + 0) += -grad; + gradient(j, i + 1) += grad; + } + else + { + /* nothing happened */ + } + } + } + + /* acceleration feasibility */ + for (int i = 0; i < q.cols() - 2; i++) + { + Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2; + + for (int j = 0; j < 3; j++) + { + if (ai(j) > max_acc_ + demarcation) + { + double diff = ai(j) - max_acc_; + cost += ar * diff * diff + br * diff + cr; + + double grad = (2.0 * ar * diff + br) * ts_inv2; + gradient(j, i + 0) += grad; + gradient(j, i + 1) += -2 * grad; + gradient(j, i + 2) += grad; + } + else if (ai(j) > max_acc_) + { + double diff = ai(j) - max_acc_; + cost += pow(diff, 3); + + double grad = 3 * diff * diff * ts_inv2; + gradient(j, i + 0) += grad; + gradient(j, i + 1) += -2 * grad; + gradient(j, i + 2) += grad; + } + else if (ai(j) < -(max_acc_ + demarcation)) + { + double diff = ai(j) + max_acc_; + cost += al * diff * diff + bl * diff + cl; + + double grad = (2.0 * al * diff + bl) * ts_inv2; + gradient(j, i + 0) += grad; + gradient(j, i + 1) += -2 * grad; + gradient(j, i + 2) += grad; + } + else if (ai(j) < -max_acc_) + { + double diff = ai(j) + max_acc_; + cost += -pow(diff, 3); + + double grad = -3 * diff * diff * ts_inv2; + gradient(j, i + 0) += grad; + gradient(j, i + 1) += -2 * grad; + gradient(j, i + 2) += grad; + } + else + { + /* nothing happened */ + } + } + } + +#else + + cost = 0.0; + /* abbreviation */ + double ts, /*vm2, am2, */ ts_inv2; + // vm2 = max_vel_ * max_vel_; + // am2 = max_acc_ * max_acc_; + + ts = bspline_interval_; + ts_inv2 = 1 / ts / ts; + + /* velocity feasibility */ + for (int i = 0; i < q.cols() - 1; i++) + { + Eigen::Vector3d vi = (q.col(i + 1) - q.col(i)) / ts; + + //cout << "temp_v * vi=" ; + for (int j = 0; j < 3; j++) + { + if (vi(j) > max_vel_) + { + // cout << "zx-todo VEL" << endl; + // cout << vi(j) << endl; + cost += pow(vi(j) - max_vel_, 2) * ts_inv2; // multiply ts_inv3 to make vel and acc has similar magnitude + + gradient(j, i + 0) += -2 * (vi(j) - max_vel_) / ts * ts_inv2; + gradient(j, i + 1) += 2 * (vi(j) - max_vel_) / ts * ts_inv2; + } + else if (vi(j) < -max_vel_) + { + cost += pow(vi(j) + max_vel_, 2) * ts_inv2; + + gradient(j, i + 0) += -2 * (vi(j) + max_vel_) / ts * ts_inv2; + gradient(j, i + 1) += 2 * (vi(j) + max_vel_) / ts * ts_inv2; + } + else + { + /* code */ + } + } + } + + /* acceleration feasibility */ + for (int i = 0; i < q.cols() - 2; i++) + { + Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2; + + //cout << "temp_a * ai=" ; + for (int j = 0; j < 3; j++) + { + if (ai(j) > max_acc_) + { + // cout << "zx-todo ACC" << endl; + // cout << ai(j) << endl; + cost += pow(ai(j) - max_acc_, 2); + + gradient(j, i + 0) += 2 * (ai(j) - max_acc_) * ts_inv2; + gradient(j, i + 1) += -4 * (ai(j) - max_acc_) * ts_inv2; + gradient(j, i + 2) += 2 * (ai(j) - max_acc_) * ts_inv2; + } + else if (ai(j) < -max_acc_) + { + cost += pow(ai(j) + max_acc_, 2); + + gradient(j, i + 0) += 2 * (ai(j) + max_acc_) * ts_inv2; + gradient(j, i + 1) += -4 * (ai(j) + max_acc_) * ts_inv2; + gradient(j, i + 2) += 2 * (ai(j) + max_acc_) * ts_inv2; + } + else + { + /* code */ + } + } + //cout << endl; + } + +#endif + } + + bool BsplineOptimizer::check_collision_and_rebound(void) + { + + int end_idx = cps_.size - order_; + + /*** Check and segment the initial trajectory according to obstacles ***/ + int in_id, out_id; + vector> segment_ids; + bool flag_new_obs_valid = false; + int i_end = end_idx - (end_idx - order_) / 3; + for (int i = order_ - 1; i <= i_end; ++i) + { + + bool occ = grid_map_->getInflateOccupancy(cps_.points.col(i)); + + /*** check if the new collision will be valid ***/ + if (occ) + { + for (size_t k = 0; k < cps_.direction[i].size(); ++k) + { + cout.precision(2); + if ((cps_.points.col(i) - cps_.base_point[i][k]).dot(cps_.direction[i][k]) < 1 * grid_map_->getResolution()) // current point is outside all the collision_points. + { + occ = false; // Not really takes effect, just for better hunman understanding. + break; + } + } + } + + if (occ) + { + flag_new_obs_valid = true; + + int j; + for (j = i - 1; j >= 0; --j) + { + occ = grid_map_->getInflateOccupancy(cps_.points.col(j)); + if (!occ) + { + in_id = j; + break; + } + } + if (j < 0) // fail to get the obs free point + { + ROS_ERROR("ERROR! the drone is in obstacle. This should not happen."); + in_id = 0; + } + + for (j = i + 1; j < cps_.size; ++j) + { + occ = grid_map_->getInflateOccupancy(cps_.points.col(j)); + + if (!occ) + { + out_id = j; + break; + } + } + if (j >= cps_.size) // fail to get the obs free point + { + ROS_WARN("WARN! terminal point of the current trajectory is in obstacle, skip this planning."); + + force_stop_type_ = STOP_FOR_ERROR; + return false; + } + + i = j + 1; + + segment_ids.push_back(std::pair(in_id, out_id)); + } + } + + if (flag_new_obs_valid) + { + vector> a_star_pathes; + for (size_t i = 0; i < segment_ids.size(); ++i) + { + /*** a star search ***/ + Eigen::Vector3d in(cps_.points.col(segment_ids[i].first)), out(cps_.points.col(segment_ids[i].second)); + if (a_star_->AstarSearch(/*(in-out).norm()/10+0.05*/ 0.1, in, out)) + { + a_star_pathes.push_back(a_star_->getPath()); + } + else + { + ROS_ERROR("a star error"); + segment_ids.erase(segment_ids.begin() + i); + i--; + } + } + + for (size_t i = 1; i < segment_ids.size(); i++) // Avoid overlap + { + if (segment_ids[i - 1].second >= segment_ids[i].first) + { + double middle = (double)(segment_ids[i - 1].second + segment_ids[i].first) / 2.0; + segment_ids[i - 1].second = static_cast(middle - 0.1); + segment_ids[i].first = static_cast(middle + 1.1); + } + } + + /*** Assign parameters to each segment ***/ + for (size_t i = 0; i < segment_ids.size(); ++i) + { + // step 1 + for (int j = segment_ids[i].first; j <= segment_ids[i].second; ++j) + cps_.flag_temp[j] = false; + + // step 2 + int got_intersection_id = -1; + for (int j = segment_ids[i].first + 1; j < segment_ids[i].second; ++j) + { + Eigen::Vector3d ctrl_pts_law(cps_.points.col(j + 1) - cps_.points.col(j - 1)), intersection_point; + int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation + double val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law), last_val = val; + while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()) + { + last_Astar_id = Astar_id; + + if (val >= 0) + --Astar_id; + else + ++Astar_id; + + val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law); + + // cout << val << endl; + + if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed + { + intersection_point = + a_star_pathes[i][Astar_id] + + ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) * + (ctrl_pts_law.dot(cps_.points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t + ); + + got_intersection_id = j; + break; + } + } + + if (got_intersection_id >= 0) + { + double length = (intersection_point - cps_.points.col(j)).norm(); + if (length > 1e-5) + { + cps_.flag_temp[j] = true; + for (double a = length; a >= 0.0; a -= grid_map_->getResolution()) + { + bool occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j)); + + if (occ || a < grid_map_->getResolution()) + { + if (occ) + a += grid_map_->getResolution(); + cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j)); + cps_.direction[j].push_back((intersection_point - cps_.points.col(j)).normalized()); + break; + } + } + } + else + { + got_intersection_id = -1; + } + } + } + + //step 3 + if (got_intersection_id >= 0) + { + for (int j = got_intersection_id + 1; j <= segment_ids[i].second; ++j) + if (!cps_.flag_temp[j]) + { + cps_.base_point[j].push_back(cps_.base_point[j - 1].back()); + cps_.direction[j].push_back(cps_.direction[j - 1].back()); + } + + for (int j = got_intersection_id - 1; j >= segment_ids[i].first; --j) + if (!cps_.flag_temp[j]) + { + cps_.base_point[j].push_back(cps_.base_point[j + 1].back()); + cps_.direction[j].push_back(cps_.direction[j + 1].back()); + } + } + else + ROS_WARN("Failed to generate direction. It doesn't matter."); + } + + force_stop_type_ = STOP_FOR_REBOUND; + return true; + } + + return false; + } + + bool BsplineOptimizer::BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double ts) + { + setBsplineInterval(ts); + + double final_cost; + bool flag_success = rebound_optimize(final_cost); + + optimal_points = cps_.points; + + return flag_success; + } + + bool BsplineOptimizer::BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double &final_cost, const ControlPoints &control_points, double ts) + { + setBsplineInterval(ts); + + cps_ = control_points; + + bool flag_success = rebound_optimize(final_cost); + + optimal_points = cps_.points; + + return flag_success; + } + + bool BsplineOptimizer::BsplineOptimizeTrajRefine(const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points) + { + + setControlPoints(init_points); + setBsplineInterval(ts); + + bool flag_success = refine_optimize(); + + optimal_points = cps_.points; + + return flag_success; + } + + bool BsplineOptimizer::rebound_optimize(double &final_cost) + { + iter_num_ = 0; + int start_id = order_; + // int end_id = this->cps_.size - order_; //Fixed end + int end_id = this->cps_.size; // Free end + variable_num_ = 3 * (end_id - start_id); + + ros::Time t0 = ros::Time::now(), t1, t2; + int restart_nums = 0, rebound_times = 0; + ; + bool flag_force_return, flag_occ, success; + new_lambda2_ = lambda2_; + constexpr int MAX_RESART_NUMS_SET = 3; + do + { + /* ---------- prepare ---------- */ + min_cost_ = std::numeric_limits::max(); + min_ellip_dist_ = INIT_min_ellip_dist_; + iter_num_ = 0; + flag_force_return = false; + flag_occ = false; + success = false; + + double q[variable_num_]; + memcpy(q, cps_.points.data() + 3 * start_id, variable_num_ * sizeof(q[0])); + + lbfgs::lbfgs_parameter_t lbfgs_params; + lbfgs::lbfgs_load_default_parameters(&lbfgs_params); + lbfgs_params.mem_size = 16; + lbfgs_params.max_iterations = 200; + lbfgs_params.g_epsilon = 0.01; + + /* ---------- optimize ---------- */ + t1 = ros::Time::now(); + int result = lbfgs::lbfgs_optimize(variable_num_, q, &final_cost, BsplineOptimizer::costFunctionRebound, NULL, BsplineOptimizer::earlyExit, this, &lbfgs_params); + t2 = ros::Time::now(); + double time_ms = (t2 - t1).toSec() * 1000; + double total_time_ms = (t2 - t0).toSec() * 1000; + + /* ---------- success temporary, check collision again ---------- */ + if (result == lbfgs::LBFGS_CONVERGENCE || + result == lbfgs::LBFGSERR_MAXIMUMITERATION || + result == lbfgs::LBFGS_ALREADY_MINIMIZED || + result == lbfgs::LBFGS_STOP) + { + //ROS_WARN("Solver error in planning!, return = %s", lbfgs::lbfgs_strerror(result)); + flag_force_return = false; + + /*** collision check, phase 1 ***/ + if ((min_ellip_dist_ != INIT_min_ellip_dist_) && (min_ellip_dist_ > swarm_clearance_)) + { + success = false; + restart_nums++; + initControlPoints(cps_.points, false); + new_lambda2_ *= 2; + + printf("\033[32miter(+1)=%d,time(ms)=%5.3f, swarm too close, keep optimizing\n\033[0m", iter_num_, time_ms); + + continue; + } + + /*** collision check, phase 2 ***/ + UniformBspline traj = UniformBspline(cps_.points, 3, bspline_interval_); + double tm, tmp; + traj.getTimeSpan(tm, tmp); + double t_step = (tmp - tm) / ((traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm)).norm() / grid_map_->getResolution()); + for (double t = tm; t < tmp * 2 / 3; t += t_step) // Only check the closest 2/3 partition of the whole trajectory. + { + flag_occ = grid_map_->getInflateOccupancy(traj.evaluateDeBoorT(t)); + if (flag_occ) + { + //cout << "hit_obs, t=" << t << " P=" << traj.evaluateDeBoorT(t).transpose() << endl; + + if (t <= bspline_interval_) // First 3 control points in obstacles! + { + // cout << cps_.points.col(1).transpose() << "\n" + // << cps_.points.col(2).transpose() << "\n" + // << cps_.points.col(3).transpose() << "\n" + // << cps_.points.col(4).transpose() << endl; + ROS_WARN("First 3 control points in obstacles! return false, t=%f", t); + return false; + } + + break; + } + } + + // cout << "XXXXXX" << ((cps_.points.col(cps_.points.cols()-1) + 4*cps_.points.col(cps_.points.cols()-2) + cps_.points.col(cps_.points.cols()-3))/6 - local_target_pt_).norm() << endl; + + /*** collision check, phase 3 ***/ +//#define USE_SECOND_CLEARENCE_CHECK +#ifdef USE_SECOND_CLEARENCE_CHECK + bool flag_cls_xyp, flag_cls_xyn, flag_cls_zp, flag_cls_zn; + Eigen::Vector3d start_end_vec = traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm); + Eigen::Vector3d offset_xy(-start_end_vec(0), start_end_vec(1), 0); + offset_xy.normalize(); + Eigen::Vector3d offset_z = start_end_vec.cross(offset_xy); + offset_z.normalize(); + offset_xy *= cps_.clearance / 2; + offset_z *= cps_.clearance / 2; + + Eigen::MatrixXd check_pts(cps_.points.rows(), cps_.points.cols()); + for (Eigen::Index i = 0; i < cps_.points.cols(); i++) + { + check_pts.col(i) = cps_.points.col(i); + check_pts(0, i) += offset_xy(0); + check_pts(1, i) += offset_xy(1); + check_pts(2, i) += offset_xy(2); + } + flag_cls_xyp = initControlPoints(check_pts, false).size() > 0; + for (Eigen::Index i = 0; i < cps_.points.cols(); i++) + { + check_pts(0, i) -= 2 * offset_xy(0); + check_pts(1, i) -= 2 * offset_xy(1); + check_pts(2, i) -= 2 * offset_xy(2); + } + flag_cls_xyn = initControlPoints(check_pts, false).size() > 0; + for (Eigen::Index i = 0; i < cps_.points.cols(); i++) + { + check_pts(0, i) += offset_xy(0) + offset_z(0); + check_pts(1, i) += offset_xy(1) + offset_z(1); + check_pts(2, i) += offset_xy(2) + offset_z(2); + } + flag_cls_zp = initControlPoints(check_pts, false).size() > 0; + for (Eigen::Index i = 0; i < cps_.points.cols(); i++) + { + check_pts(0, i) -= 2 * offset_z(0); + check_pts(1, i) -= 2 * offset_z(1); + check_pts(2, i) -= 2 * offset_z(2); + } + flag_cls_zn = initControlPoints(check_pts, false).size() > 0; + if ((flag_cls_xyp ^ flag_cls_xyn) || (flag_cls_zp ^ flag_cls_zn)) + flag_occ = true; +#endif + + if (!flag_occ) + { + printf("\033[32miter(+1)=%d,time(ms)=%5.3f,total_t(ms)=%5.3f,cost=%5.3f\n\033[0m", iter_num_, time_ms, total_time_ms, final_cost); + success = true; + } + else // restart + { + restart_nums++; + initControlPoints(cps_.points, false); + new_lambda2_ *= 2; + + printf("\033[32miter(+1)=%d,time(ms)=%5.3f, collided, keep optimizing\n\033[0m", iter_num_, time_ms); + } + } + else if (result == lbfgs::LBFGSERR_CANCELED) + { + flag_force_return = true; + rebound_times++; + cout << "iter=" << iter_num_ << ",time(ms)=" << time_ms << ",rebound." << endl; + } + else + { + ROS_WARN("Solver error. Return = %d, %s. Skip this planning.", result, lbfgs::lbfgs_strerror(result)); + // while (ros::ok()); + } + + } while ( + ((flag_occ || ((min_ellip_dist_ != INIT_min_ellip_dist_) && (min_ellip_dist_ > swarm_clearance_))) && restart_nums < MAX_RESART_NUMS_SET) || + (flag_force_return && force_stop_type_ == STOP_FOR_REBOUND && rebound_times <= 20)); + + return success; + } + + bool BsplineOptimizer::refine_optimize() + { + iter_num_ = 0; + int start_id = order_; + int end_id = this->cps_.points.cols() - order_; + variable_num_ = 3 * (end_id - start_id); + + double q[variable_num_]; + double final_cost; + + memcpy(q, cps_.points.data() + 3 * start_id, variable_num_ * sizeof(q[0])); + + double origin_lambda4 = lambda4_; + bool flag_safe = true; + int iter_count = 0; + do + { + lbfgs::lbfgs_parameter_t lbfgs_params; + lbfgs::lbfgs_load_default_parameters(&lbfgs_params); + lbfgs_params.mem_size = 16; + lbfgs_params.max_iterations = 200; + lbfgs_params.g_epsilon = 0.001; + + int result = lbfgs::lbfgs_optimize(variable_num_, q, &final_cost, BsplineOptimizer::costFunctionRefine, NULL, NULL, this, &lbfgs_params); + if (result == lbfgs::LBFGS_CONVERGENCE || + result == lbfgs::LBFGSERR_MAXIMUMITERATION || + result == lbfgs::LBFGS_ALREADY_MINIMIZED || + result == lbfgs::LBFGS_STOP) + { + //pass + } + else + { + ROS_ERROR("Solver error in refining!, return = %d, %s", result, lbfgs::lbfgs_strerror(result)); + } + + UniformBspline traj = UniformBspline(cps_.points, 3, bspline_interval_); + double tm, tmp; + traj.getTimeSpan(tm, tmp); + double t_step = (tmp - tm) / ((traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm)).norm() / grid_map_->getResolution()); // Step size is defined as the maximum size that can passes throgth every gird. + for (double t = tm; t < tmp * 2 / 3; t += t_step) + { + if (grid_map_->getInflateOccupancy(traj.evaluateDeBoorT(t))) + { + // cout << "Refined traj hit_obs, t=" << t << " P=" << traj.evaluateDeBoorT(t).transpose() << endl; + + Eigen::MatrixXd ref_pts(ref_pts_.size(), 3); + for (size_t i = 0; i < ref_pts_.size(); i++) + { + ref_pts.row(i) = ref_pts_[i].transpose(); + } + + flag_safe = false; + break; + } + } + + if (!flag_safe) + lambda4_ *= 2; + + iter_count++; + } while (!flag_safe && iter_count <= 0); + + lambda4_ = origin_lambda4; + + //cout << "iter_num_=" << iter_num_ << endl; + + return flag_safe; + } + + void BsplineOptimizer::combineCostRebound(const double *x, double *grad, double &f_combine, const int n) + { + // cout << "drone_id_=" << drone_id_ << endl; + // cout << "cps_.points.size()=" << cps_.points.size() << endl; + // cout << "n=" << n << endl; + // cout << "sizeof(x[0])=" << sizeof(x[0]) << endl; + + memcpy(cps_.points.data() + 3 * order_, x, n * sizeof(x[0])); + + /* ---------- evaluate cost and gradient ---------- */ + double f_smoothness, f_distance, f_feasibility /*, f_mov_objs*/, f_swarm, f_terminal; + + Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.size); + Eigen::MatrixXd g_distance = Eigen::MatrixXd::Zero(3, cps_.size); + Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.size); + // Eigen::MatrixXd g_mov_objs = Eigen::MatrixXd::Zero(3, cps_.size); + Eigen::MatrixXd g_swarm = Eigen::MatrixXd::Zero(3, cps_.size); + Eigen::MatrixXd g_terminal = Eigen::MatrixXd::Zero(3, cps_.size); + + calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness); + calcDistanceCostRebound(cps_.points, f_distance, g_distance, iter_num_, f_smoothness); + calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility); + // calcMovingObjCost(cps_.points, f_mov_objs, g_mov_objs); + calcSwarmCost(cps_.points, f_swarm, g_swarm); + calcTerminalCost( cps_.points, f_terminal, g_terminal ); + + f_combine = lambda1_ * f_smoothness + new_lambda2_ * f_distance + lambda3_ * f_feasibility + new_lambda2_ * f_swarm + lambda2_ * f_terminal; + //f_combine = lambda1_ * f_smoothness + new_lambda2_ * f_distance + lambda3_ * f_feasibility + new_lambda2_ * f_mov_objs; + //printf("origin %f %f %f %f\n", f_smoothness, f_distance, f_feasibility, f_combine); + + Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + new_lambda2_ * g_distance + lambda3_ * g_feasibility + new_lambda2_ * g_swarm + lambda2_ * g_terminal; + //Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + new_lambda2_ * g_distance + lambda3_ * g_feasibility + new_lambda2_ * g_mov_objs; + memcpy(grad, grad_3D.data() + 3 * order_, n * sizeof(grad[0])); + } + + void BsplineOptimizer::combineCostRefine(const double *x, double *grad, double &f_combine, const int n) + { + + memcpy(cps_.points.data() + 3 * order_, x, n * sizeof(x[0])); + + /* ---------- evaluate cost and gradient ---------- */ + double f_smoothness, f_fitness, f_feasibility; + + Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.points.cols()); + Eigen::MatrixXd g_fitness = Eigen::MatrixXd::Zero(3, cps_.points.cols()); + Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.points.cols()); + + //time_satrt = ros::Time::now(); + + calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness); + calcFitnessCost(cps_.points, f_fitness, g_fitness); + calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility); + + /* ---------- convert to solver format...---------- */ + f_combine = lambda1_ * f_smoothness + lambda4_ * f_fitness + lambda3_ * f_feasibility; + // printf("origin %f %f %f %f\n", f_smoothness, f_fitness, f_feasibility, f_combine); + + Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + lambda4_ * g_fitness + lambda3_ * g_feasibility; + memcpy(grad, grad_3D.data() + 3 * order_, n * sizeof(grad[0])); + } + +} // namespace ego_planner \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/bspline_opt/src/gradient_descent_optimizer.cpp b/motion_planning/3d/ego_planner/planner/bspline_opt/src/gradient_descent_optimizer.cpp new file mode 100755 index 0000000..c3371d6 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/bspline_opt/src/gradient_descent_optimizer.cpp @@ -0,0 +1,94 @@ +#include + +#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; +} diff --git a/motion_planning/3d/ego_planner/planner/bspline_opt/src/uniform_bspline.cpp b/motion_planning/3d/ego_planner/planner/bspline_opt/src/uniform_bspline.cpp new file mode 100644 index 0000000..ff798f0 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/bspline_opt/src/uniform_bspline.cpp @@ -0,0 +1,377 @@ +#include "bspline_opt/uniform_bspline.h" +#include + +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 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 &point_set, + const vector &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 diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/CMakeLists.txt b/motion_planning/3d/ego_planner/planner/drone_detect/CMakeLists.txt new file mode 100644 index 0000000..1eacae4 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/CMakeLists.txt @@ -0,0 +1,130 @@ +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() diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/LICENSE b/motion_planning/3d/ego_planner/planner/drone_detect/LICENSE new file mode 100644 index 0000000..9b02d46 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/LICENSE @@ -0,0 +1,29 @@ +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. diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/README.md b/motion_planning/3d/ego_planner/planner/drone_detect/README.md new file mode 100644 index 0000000..2388e52 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/README.md @@ -0,0 +1,97 @@ +# 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 + + + + + + + + + + + + + + +``` + + + +## 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. \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/config/camera.yaml b/motion_planning/3d/ego_planner/planner/drone_detect/config/camera.yaml new file mode 100755 index 0000000..556e596 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/config/camera.yaml @@ -0,0 +1,7 @@ +cam_width: 640 +cam_height: 480 +cam_fx: 386.02557373046875 +cam_fy: 386.02557373046875 +cam_cx: 321.8554382324219 +cam_cy: 241.2396240234375 + diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/config/default.yaml b/motion_planning/3d/ego_planner/planner/drone_detect/config/default.yaml new file mode 100644 index 0000000..37a666d --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/config/default.yaml @@ -0,0 +1,5 @@ +# debug_flag: true +pixel_ratio: 0.1 +estimate/max_pose_error: 0.4 +estimate/drone_width: 0.5 +estimate/drone_height: 0.2 \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/doc/demo.jpg b/motion_planning/3d/ego_planner/planner/drone_detect/doc/demo.jpg new file mode 100644 index 0000000..690972d Binary files /dev/null and b/motion_planning/3d/ego_planner/planner/drone_detect/doc/demo.jpg differ diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/doc/example.jpg b/motion_planning/3d/ego_planner/planner/drone_detect/doc/example.jpg new file mode 100644 index 0000000..d047c4f Binary files /dev/null and b/motion_planning/3d/ego_planner/planner/drone_detect/doc/example.jpg differ diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/include/drone_detector/drone_detector.h b/motion_planning/3d/ego_planner/planner/drone_detect/include/drone_detector/drone_detector.h new file mode 100644 index 0000000..271210e --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/include/drone_detector/drone_detector.h @@ -0,0 +1,156 @@ +#pragma once +#include +#include +// ROS +#include +#include "std_msgs/String.h" +#include "std_msgs/Bool.h" +#include +#include +#include +// synchronize topic +#include +#include +#include +#include + +#include + +//include opencv and eigen +#include +#include +#include +#include +#include + +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 SyncPolicyDepthColorImagePose; + typedef std::shared_ptr> SynchronizerDepthColorImagePose; + typedef message_filters::sync_policies::ApproximateTime SyncPolicyDepthImagePose; + typedef std::shared_ptr> SynchronizerDepthImagePose; + + // std::shared_ptr> depth_img_sub_; + std::shared_ptr> colordepth_img_sub_; + std::shared_ptr> 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 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 */ diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/launch/drone_detect.launch b/motion_planning/3d/ego_planner/planner/drone_detect/launch/drone_detect.launch new file mode 100644 index 0000000..6aa4a25 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/launch/drone_detect.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/launch/ros_package_template_overlying_params.launch b/motion_planning/3d/ego_planner/planner/drone_detect/launch/ros_package_template_overlying_params.launch new file mode 100644 index 0000000..0ac3e22 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/launch/ros_package_template_overlying_params.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/package.xml b/motion_planning/3d/ego_planner/planner/drone_detect/package.xml new file mode 100644 index 0000000..bef3270 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/package.xml @@ -0,0 +1,20 @@ + + + drone_detect + 0.1.0 + Detect other drones in depth image. + Jiangchao Zhu + BSD + Jiangchao Zhu + + + catkin + + boost + + eigen + roscpp + sensor_msgs + + roslint + diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/src/drone_detect_node.cpp b/motion_planning/3d/ego_planner/planner/drone_detect/src/drone_detect_node.cpp new file mode 100644 index 0000000..8e29362 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/src/drone_detect_node.cpp @@ -0,0 +1,14 @@ +#include +#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; +} diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/src/drone_detector.cpp b/motion_planning/3d/ego_planner/planner/drone_detect/src/drone_detector.cpp new file mode 100644 index 0000000..d3bacd3 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/src/drone_detector.cpp @@ -0,0 +1,423 @@ +#include "drone_detector/drone_detector.h" + +// STD +#include + +namespace detect { + +DroneDetector::DroneDetector(ros::NodeHandle& nodeHandle) + : nh_(nodeHandle) +{ + readParameters(); + + // depth_img_sub_.reset(new message_filters::Subscriber(nh_, "depth", 50, ros::TransportHints().tcpNoDelay())); + // colordepth_img_sub_.reset(new message_filters::Subscriber(nh_, "colordepth", 50)); + // camera_pos_sub_.reset(new message_filters::Subscriber(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("new_depth_image", 50); + debug_depth_img_pub_ = nh_.advertise("debug_depth_image", 50); + + debug_info_pub_ = nh_.advertise("/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("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(hit_pixels_[i][k](1), hit_pixels_[i][k](0)) = 0; + uint16_t *row_ptr; + row_ptr = depth_img_.ptr(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(tmp_pixel(1), tmp_pixel(0)); + uint16_t *row_ptr; + row_ptr = depth_img_.ptr(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(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(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(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(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 */ diff --git a/motion_planning/3d/ego_planner/planner/drone_detect/test/test_drone_detector.cpp b/motion_planning/3d/ego_planner/planner/drone_detect/test/test_drone_detector.cpp new file mode 100644 index 0000000..2fc9dd5 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/drone_detect/test/test_drone_detector.cpp @@ -0,0 +1,10 @@ +// gtest +#include + +// 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(); +} \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/path_searching/CMakeLists.txt b/motion_planning/3d/ego_planner/planner/path_searching/CMakeLists.txt new file mode 100755 index 0000000..13f1f66 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/path_searching/CMakeLists.txt @@ -0,0 +1,42 @@ +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} + ) diff --git a/motion_planning/3d/ego_planner/planner/path_searching/include/path_searching/dyn_a_star.h b/motion_planning/3d/ego_planner/planner/path_searching/include/path_searching/dyn_a_star.h new file mode 100755 index 0000000..388668b --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/path_searching/include/path_searching/dyn_a_star.h @@ -0,0 +1,115 @@ +#ifndef _DYN_A_STAR_H_ +#define _DYN_A_STAR_H_ + +#include +#include +#include +#include +#include +#include + +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 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 gridPath_; + + GridNodePtr ***GridNodeMap_; + std::priority_queue, NodeComparator> openSet_; + + int rounds_{0}; + +public: + typedef std::shared_ptr 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 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() * 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() + 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 diff --git a/motion_planning/3d/ego_planner/planner/path_searching/package.xml b/motion_planning/3d/ego_planner/planner/path_searching/package.xml new file mode 100755 index 0000000..0394974 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/path_searching/package.xml @@ -0,0 +1,71 @@ + + + path_searching + 0.0.0 + The path_searching package + + + + + iszhouxin + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + plan_env + roscpp + rospy + std_msgs + plan_env + roscpp + rospy + std_msgs + plan_env + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/path_searching/src/dyn_a_star.cpp b/motion_planning/3d/ego_planner/planner/path_searching/src/dyn_a_star.cpp new file mode 100755 index 0000000..744daa9 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/path_searching/src/dyn_a_star.cpp @@ -0,0 +1,260 @@ +#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 AStar::retrievePath(GridNodePtr current) +{ + vector 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, 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 AStar::getPath() +{ + vector path; + + for (auto ptr : gridPath_) + path.push_back(Index2Coord(ptr->index)); + + reverse(path.begin(), path.end()); + return path; +} diff --git a/motion_planning/3d/ego_planner/planner/plan_env/CMakeLists.txt b/motion_planning/3d/ego_planner/planner/plan_env/CMakeLists.txt new file mode 100755 index 0000000..1567f41 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_env/CMakeLists.txt @@ -0,0 +1,57 @@ +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 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} + ) + +add_executable(obj_generator + src/obj_generator.cpp +) +target_link_libraries(obj_generator + ${catkin_LIBRARIES} + ) diff --git a/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/grid_map.h b/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/grid_map.h new file mode 100644 index 0000000..bba2603 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/grid_map.h @@ -0,0 +1,804 @@ +#ifndef _GRID_MAP_H +#define _GRID_MAP_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#define logit(x) (log((x) / (1 - (x)))) + +using namespace std; + +// voxel hashing +template +struct matrix_hash : std::unary_function { + 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()(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 occupancy_buffer_; + std::vector 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 proj_points_; + int proj_points_cnt; + + // flag buffers for speeding up raycasting + + vector count_hit_, count_hit_and_miss_; + vector flag_traverse_, flag_rayend_; + char raycast_num_; + queue 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 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& 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 SyncPolicyImageOdom; typedef + // message_filters::sync_policies::ExactTime SyncPolicyImagePose; + typedef message_filters::sync_policies::ApproximateTime + SyncPolicyImageOdom; + typedef message_filters::sync_policies::ApproximateTime + SyncPolicyImagePose; + typedef shared_ptr> SynchronizerImagePose; + typedef shared_ptr> SynchronizerImageOdom; + + ros::NodeHandle node_; + shared_ptr> depth_sub_; + shared_ptr> pose_sub_; + shared_ptr> 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 rand_noise_; + normal_distribution 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& 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 +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// #include +// #include +// #include + +// #include +// #include +// #include +// #include + +// #include + +// #define logit(x) (log((x) / (1 - (x)))) + +// using namespace std; + +// // voxel hashing +// template +// struct matrix_hash : std::unary_function { +// 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()(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 occupancy_buffer_; +// std::vector 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 proj_points_; +// int proj_points_cnt; + +// // flag buffers for speeding up raycasting + +// vector count_hit_, count_hit_and_miss_; +// vector flag_traverse_, flag_rayend_; +// char raycast_num_; +// queue 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 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& 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 SyncPolicyImageOdom; typedef +// // message_filters::sync_policies::ExactTime SyncPolicyImagePose; +// typedef message_filters::sync_policies::ApproximateTime +// SyncPolicyImageOdom; +// typedef message_filters::sync_policies::ApproximateTime +// SyncPolicyImagePose; +// typedef shared_ptr> SynchronizerImagePose; +// typedef shared_ptr> SynchronizerImageOdom; + +// ros::NodeHandle node_; +// shared_ptr> depth_sub_; +// shared_ptr> pose_sub_; +// shared_ptr> 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 rand_noise_; +// normal_distribution 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& 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 \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/linear_obj_model.hpp b/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/linear_obj_model.hpp new file mode 100644 index 0000000..574e3b0 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/linear_obj_model.hpp @@ -0,0 +1,267 @@ +/** +* This file is part of Fast-Planner. +* +* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, +* Developed by Boyu Zhou , +* for more information see . +* 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 . +*/ + + + +#ifndef _LINEAR_OBJ_MODEL_H_ +#define _LINEAR_OBJ_MODEL_H_ + +#include +#include +#include + +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 \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/obj_predictor.h b/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/obj_predictor.h new file mode 100644 index 0000000..fb2e86b --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/obj_predictor.h @@ -0,0 +1,176 @@ +/** +* This file is part of Fast-Planner. +* +* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, +* Developed by Boyu Zhou , +* for more information see . +* 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 . +*/ + + + +#ifndef _OBJ_PREDICTOR_H_ +#define _OBJ_PREDICTOR_H_ + +#include +#include +#include +#include +#include +#include +#include + +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> ObjPrediction; +typedef shared_ptr> ObjScale; + +/* ========== prediction polynomial ========== */ +class PolynomialPrediction { +private: + vector> polys; + double t1, t2; // start / end + ros::Time global_start_time_; + +public: + PolynomialPrediction(/* args */) { + } + ~PolynomialPrediction() { + } + + void setPolynomial(vector>& 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 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 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& his) { + his = history_; + } + +private: + list 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 pose_subs_; + ros::Subscriber marker_sub_; + ros::Timer predict_timer_; + vector> obj_histories_; + + /* share data with planner */ + ObjPrediction predict_trajs_; + ObjScale obj_scale_; + vector 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 Ptr; +}; + +} // namespace fast_planner + +#endif \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/raycast.h b/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/raycast.h new file mode 100644 index 0000000..23e0876 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_env/include/plan_env/raycast.h @@ -0,0 +1,63 @@ +#ifndef RAYCAST_H_ +#define RAYCAST_H_ + +#include +#include + +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* 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_ \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/plan_env/package.xml b/motion_planning/3d/ego_planner/planner/plan_env/package.xml new file mode 100755 index 0000000..7a55ca1 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_env/package.xml @@ -0,0 +1,68 @@ + + + plan_env + 0.0.0 + The plan_env package + + + + + iszhouxin + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_env/src/grid_map.cpp b/motion_planning/3d/ego_planner/planner/plan_env/src/grid_map.cpp new file mode 100644 index 0000000..73a751e --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_env/src/grid_map.cpp @@ -0,0 +1,1019 @@ +#include "plan_env/grid_map.h" + +// #define current_img_ md_.depth_image_[image_cnt_ & 1] +// #define last_img_ md_.depth_image_[!(image_cnt_ & 1)] + +void GridMap::initMap(ros::NodeHandle &nh) +{ + node_ = nh; + + /* get parameter */ + double x_size, y_size, z_size; + node_.param("grid_map/resolution", mp_.resolution_, -1.0); + node_.param("grid_map/map_size_x", x_size, -1.0); + node_.param("grid_map/map_size_y", y_size, -1.0); + node_.param("grid_map/map_size_z", z_size, -1.0); + node_.param("grid_map/local_update_range_x", mp_.local_update_range_(0), -1.0); + node_.param("grid_map/local_update_range_y", mp_.local_update_range_(1), -1.0); + node_.param("grid_map/local_update_range_z", mp_.local_update_range_(2), -1.0); + node_.param("grid_map/obstacles_inflation", mp_.obstacles_inflation_, -1.0); + + node_.param("grid_map/fx", mp_.fx_, -1.0); + node_.param("grid_map/fy", mp_.fy_, -1.0); + node_.param("grid_map/cx", mp_.cx_, -1.0); + node_.param("grid_map/cy", mp_.cy_, -1.0); + + node_.param("grid_map/use_depth_filter", mp_.use_depth_filter_, true); + node_.param("grid_map/depth_filter_tolerance", mp_.depth_filter_tolerance_, -1.0); + node_.param("grid_map/depth_filter_maxdist", mp_.depth_filter_maxdist_, -1.0); + node_.param("grid_map/depth_filter_mindist", mp_.depth_filter_mindist_, -1.0); + node_.param("grid_map/depth_filter_margin", mp_.depth_filter_margin_, -1); + node_.param("grid_map/k_depth_scaling_factor", mp_.k_depth_scaling_factor_, -1.0); + node_.param("grid_map/skip_pixel", mp_.skip_pixel_, -1); + + node_.param("grid_map/p_hit", mp_.p_hit_, 0.70); + node_.param("grid_map/p_miss", mp_.p_miss_, 0.35); + node_.param("grid_map/p_min", mp_.p_min_, 0.12); + node_.param("grid_map/p_max", mp_.p_max_, 0.97); + node_.param("grid_map/p_occ", mp_.p_occ_, 0.80); + node_.param("grid_map/min_ray_length", mp_.min_ray_length_, -0.1); + node_.param("grid_map/max_ray_length", mp_.max_ray_length_, -0.1); + + node_.param("grid_map/visualization_truncate_height", mp_.visualization_truncate_height_, -0.1); + node_.param("grid_map/virtual_ceil_height", mp_.virtual_ceil_height_, -0.1); + node_.param("grid_map/virtual_ceil_yp", mp_.virtual_ceil_yp_, -0.1); + node_.param("grid_map/virtual_ceil_yn", mp_.virtual_ceil_yn_, -0.1); + + node_.param("grid_map/show_occ_time", mp_.show_occ_time_, false); + node_.param("grid_map/pose_type", mp_.pose_type_, 1); + + node_.param("grid_map/frame_id", mp_.frame_id_, string("world")); + node_.param("grid_map/local_map_margin", mp_.local_map_margin_, 1); + node_.param("grid_map/ground_height", mp_.ground_height_, 1.0); + + node_.param("grid_map/odom_depth_timeout", mp_.odom_depth_timeout_, 1.0); + + mp_.resolution_inv_ = 1 / mp_.resolution_; + mp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_); + mp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size); + + mp_.prob_hit_log_ = logit(mp_.p_hit_); + mp_.prob_miss_log_ = logit(mp_.p_miss_); + mp_.clamp_min_log_ = logit(mp_.p_min_); + mp_.clamp_max_log_ = logit(mp_.p_max_); + mp_.min_occupancy_log_ = logit(mp_.p_occ_); + mp_.unknown_flag_ = 0.01; + + cout << "hit: " << mp_.prob_hit_log_ << endl; + cout << "miss: " << mp_.prob_miss_log_ << endl; + cout << "min log: " << mp_.clamp_min_log_ << endl; + cout << "max: " << mp_.clamp_max_log_ << endl; + cout << "thresh log: " << mp_.min_occupancy_log_ << endl; + + for (int i = 0; i < 3; ++i) + mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_); + + mp_.map_min_boundary_ = mp_.map_origin_; + mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_; + + // initialize data buffers + + int buffer_size = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2); + + md_.occupancy_buffer_ = vector(buffer_size, mp_.clamp_min_log_ - mp_.unknown_flag_); + md_.occupancy_buffer_inflate_ = vector(buffer_size, 0); + + md_.count_hit_and_miss_ = vector(buffer_size, 0); + md_.count_hit_ = vector(buffer_size, 0); + md_.flag_rayend_ = vector(buffer_size, -1); + md_.flag_traverse_ = vector(buffer_size, -1); + + md_.raycast_num_ = 0; + + md_.proj_points_.resize(640 * 480 / mp_.skip_pixel_ / mp_.skip_pixel_); + md_.proj_points_cnt = 0; + + md_.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 callback */ + + depth_sub_.reset(new message_filters::Subscriber(node_, "grid_map/depth", 50)); + extrinsic_sub_ = node_.subscribe( + "/vins_estimator/extrinsic", 10, &GridMap::extrinsicCallback, this); //sub + + if (mp_.pose_type_ == POSE_STAMPED) + { + pose_sub_.reset( + new message_filters::Subscriber(node_, "grid_map/pose", 25)); + + sync_image_pose_.reset(new message_filters::Synchronizer( + SyncPolicyImagePose(100), *depth_sub_, *pose_sub_)); + sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2)); + } + else if (mp_.pose_type_ == ODOMETRY) + { + odom_sub_.reset(new message_filters::Subscriber(node_, "grid_map/odom", 100, ros::TransportHints().tcpNoDelay())); + + sync_image_odom_.reset(new message_filters::Synchronizer( + SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_)); + sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2)); + } + + // use odometry and point cloud + indep_cloud_sub_ = + node_.subscribe("grid_map/cloud", 10, &GridMap::cloudCallback, this); + indep_odom_sub_ = + node_.subscribe("grid_map/odom", 10, &GridMap::odomCallback, this); + + occ_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::updateOccupancyCallback, this); + vis_timer_ = node_.createTimer(ros::Duration(0.11), &GridMap::visCallback, this); + + map_pub_ = node_.advertise("grid_map/occupancy", 10); + map_inf_pub_ = node_.advertise("grid_map/occupancy_inflate", 10); + + md_.occ_need_update_ = false; + md_.local_updated_ = false; + md_.has_first_depth_ = false; + md_.has_odom_ = false; + md_.has_cloud_ = false; + md_.image_cnt_ = 0; + md_.last_occ_update_time_.fromSec(0); + + md_.fuse_time_ = 0.0; + md_.update_num_ = 0; + md_.max_fuse_time_ = 0.0; + + md_.flag_depth_odom_timeout_ = false; + md_.flag_use_depth_fusion = false; + + // rand_noise_ = uniform_real_distribution(-0.2, 0.2); + // rand_noise2_ = normal_distribution(0, 0.2); + // random_device rd; + // eng_ = default_random_engine(rd()); +} + +void GridMap::resetBuffer() +{ + Eigen::Vector3d min_pos = mp_.map_min_boundary_; + Eigen::Vector3d max_pos = mp_.map_max_boundary_; + + resetBuffer(min_pos, max_pos); + + md_.local_bound_min_ = Eigen::Vector3i::Zero(); + md_.local_bound_max_ = mp_.map_voxel_num_ - Eigen::Vector3i::Ones(); +} + +void GridMap::resetBuffer(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos) +{ + + Eigen::Vector3i min_id, max_id; + posToIndex(min_pos, min_id); + posToIndex(max_pos, max_id); + + boundIndex(min_id); + boundIndex(max_id); + + /* reset occ and dist buffer */ + for (int x = min_id(0); x <= max_id(0); ++x) + for (int y = min_id(1); y <= max_id(1); ++y) + for (int z = min_id(2); z <= max_id(2); ++z) + { + md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0; + } +} + +int GridMap::setCacheOccupancy(Eigen::Vector3d pos, int occ) +{ + if (occ != 1 && occ != 0) + return INVALID_IDX; + + Eigen::Vector3i id; + posToIndex(pos, id); + int idx_ctns = toAddress(id); + + md_.count_hit_and_miss_[idx_ctns] += 1; + + if (md_.count_hit_and_miss_[idx_ctns] == 1) + { + md_.cache_voxel_.push(id); + } + + if (occ == 1) + md_.count_hit_[idx_ctns] += 1; + + return idx_ctns; +} + +void GridMap::projectDepthImage() +{ + // md_.proj_points_.clear(); + md_.proj_points_cnt = 0; + + uint16_t *row_ptr; + // int cols = current_img_.cols, rows = current_img_.rows; + int cols = md_.depth_image_.cols; + int rows = md_.depth_image_.rows; + int skip_pix = mp_.skip_pixel_; + + double depth; + + Eigen::Matrix3d camera_r = md_.camera_r_m_; + + if (!mp_.use_depth_filter_) + { + for (int v = 0; v < rows; v+=skip_pix) + { + row_ptr = md_.depth_image_.ptr(v); + + for (int u = 0; u < cols; u+=skip_pix) + { + + Eigen::Vector3d proj_pt; + depth = (*row_ptr++) / mp_.k_depth_scaling_factor_; + proj_pt(0) = (u - mp_.cx_) * depth / mp_.fx_; + proj_pt(1) = (v - mp_.cy_) * depth / mp_.fy_; + proj_pt(2) = depth; + + proj_pt = camera_r * proj_pt + md_.camera_pos_; + + if (u == 320 && v == 240) + std::cout << "depth: " << depth << std::endl; + md_.proj_points_[md_.proj_points_cnt++] = proj_pt; + } + } + } + /* use depth filter */ + else + { + + if (!md_.has_first_depth_) + md_.has_first_depth_ = true; + else + { + Eigen::Vector3d pt_cur, pt_world, pt_reproj; + + Eigen::Matrix3d last_camera_r_inv; + last_camera_r_inv = md_.last_camera_r_m_.inverse(); + const double inv_factor = 1.0 / mp_.k_depth_scaling_factor_; + + for (int v = mp_.depth_filter_margin_; v < rows - mp_.depth_filter_margin_; v += mp_.skip_pixel_) + { + row_ptr = md_.depth_image_.ptr(v) + mp_.depth_filter_margin_; + + for (int u = mp_.depth_filter_margin_; u < cols - mp_.depth_filter_margin_; + u += mp_.skip_pixel_) + { + + depth = (*row_ptr) * inv_factor; + row_ptr = row_ptr + mp_.skip_pixel_; + + // filter depth + // depth += rand_noise_(eng_); + // if (depth > 0.01) depth += rand_noise2_(eng_); + + if (*row_ptr == 0) + { + depth = mp_.max_ray_length_ + 0.1; + } + else if (depth < mp_.depth_filter_mindist_) + { + continue; + } + else if (depth > mp_.depth_filter_maxdist_) + { + depth = mp_.max_ray_length_ + 0.1; + } + + // project to world frame + pt_cur(0) = (u - mp_.cx_) * depth / mp_.fx_; + pt_cur(1) = (v - mp_.cy_) * depth / mp_.fy_; + pt_cur(2) = depth; + + pt_world = camera_r * pt_cur + md_.camera_pos_; + // if (!isInMap(pt_world)) { + // pt_world = closetPointInMap(pt_world, md_.camera_pos_); + // } + + md_.proj_points_[md_.proj_points_cnt++] = pt_world; + + // check consistency with last image, disabled... + if (false) + { + pt_reproj = last_camera_r_inv * (pt_world - md_.last_camera_pos_); + double uu = pt_reproj.x() * mp_.fx_ / pt_reproj.z() + mp_.cx_; + double vv = pt_reproj.y() * mp_.fy_ / pt_reproj.z() + mp_.cy_; + + if (uu >= 0 && uu < cols && vv >= 0 && vv < rows) + { + if (fabs(md_.last_depth_image_.at((int)vv, (int)uu) * inv_factor - + pt_reproj.z()) < mp_.depth_filter_tolerance_) + { + md_.proj_points_[md_.proj_points_cnt++] = pt_world; + } + } + else + { + md_.proj_points_[md_.proj_points_cnt++] = pt_world; + } + } + } + } + } + } + + /* maintain camera pose for consistency check */ + + md_.last_camera_pos_ = md_.camera_pos_; + md_.last_camera_r_m_ = md_.camera_r_m_; + md_.last_depth_image_ = md_.depth_image_; +} + +void GridMap::raycastProcess() +{ + // if (md_.proj_points_.size() == 0) + if (md_.proj_points_cnt == 0) + return; + + ros::Time t1, t2; + + md_.raycast_num_ += 1; + + int vox_idx; + double length; + + // bounding box of updated region + double min_x = mp_.map_max_boundary_(0); + double min_y = mp_.map_max_boundary_(1); + double min_z = mp_.map_max_boundary_(2); + + double max_x = mp_.map_min_boundary_(0); + double max_y = mp_.map_min_boundary_(1); + double max_z = mp_.map_min_boundary_(2); + + RayCaster raycaster; + Eigen::Vector3d half = Eigen::Vector3d(0.5, 0.5, 0.5); + Eigen::Vector3d ray_pt, pt_w; + + for (int i = 0; i < md_.proj_points_cnt; ++i) + { + pt_w = md_.proj_points_[i]; + + // set flag for projected point + + if (!isInMap(pt_w)) + { + pt_w = closetPointInMap(pt_w, md_.camera_pos_); + + length = (pt_w - md_.camera_pos_).norm(); + if (length > mp_.max_ray_length_) + { + pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_; + } + vox_idx = setCacheOccupancy(pt_w, 0); + } + else + { + length = (pt_w - md_.camera_pos_).norm(); + + if (length > mp_.max_ray_length_) + { + pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_; + vox_idx = setCacheOccupancy(pt_w, 0); + } + else + { + vox_idx = setCacheOccupancy(pt_w, 1); + } + } + + max_x = max(max_x, pt_w(0)); + max_y = max(max_y, pt_w(1)); + max_z = max(max_z, pt_w(2)); + + min_x = min(min_x, pt_w(0)); + min_y = min(min_y, pt_w(1)); + min_z = min(min_z, pt_w(2)); + + // raycasting between camera center and point + + if (vox_idx != INVALID_IDX) + { + if (md_.flag_rayend_[vox_idx] == md_.raycast_num_) + { + continue; + } + else + { + md_.flag_rayend_[vox_idx] = md_.raycast_num_; + } + } + + raycaster.setInput(pt_w / mp_.resolution_, md_.camera_pos_ / mp_.resolution_); + + while (raycaster.step(ray_pt)) + { + Eigen::Vector3d tmp = (ray_pt + half) * mp_.resolution_; + length = (tmp - md_.camera_pos_).norm(); + + // if (length < mp_.min_ray_length_) break; + + vox_idx = setCacheOccupancy(tmp, 0); + + if (vox_idx != INVALID_IDX) + { + if (md_.flag_traverse_[vox_idx] == md_.raycast_num_) + { + break; + } + else + { + md_.flag_traverse_[vox_idx] = md_.raycast_num_; + } + } + } + } + + min_x = min(min_x, md_.camera_pos_(0)); + min_y = min(min_y, md_.camera_pos_(1)); + min_z = min(min_z, md_.camera_pos_(2)); + + max_x = max(max_x, md_.camera_pos_(0)); + max_y = max(max_y, md_.camera_pos_(1)); + max_z = max(max_z, md_.camera_pos_(2)); + max_z = max(max_z, mp_.ground_height_); + + posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_); + posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_); + boundIndex(md_.local_bound_min_); + boundIndex(md_.local_bound_max_); + + md_.local_updated_ = true; + + // update occupancy cached in queue + Eigen::Vector3d local_range_min = md_.camera_pos_ - mp_.local_update_range_; + Eigen::Vector3d local_range_max = md_.camera_pos_ + mp_.local_update_range_; + + Eigen::Vector3i min_id, max_id; + posToIndex(local_range_min, min_id); + posToIndex(local_range_max, max_id); + boundIndex(min_id); + boundIndex(max_id); + + // std::cout << "cache all: " << md_.cache_voxel_.size() << std::endl; + + while (!md_.cache_voxel_.empty()) + { + + Eigen::Vector3i idx = md_.cache_voxel_.front(); + int idx_ctns = toAddress(idx); + md_.cache_voxel_.pop(); + + double log_odds_update = + md_.count_hit_[idx_ctns] >= md_.count_hit_and_miss_[idx_ctns] - md_.count_hit_[idx_ctns] ? mp_.prob_hit_log_ : mp_.prob_miss_log_; + + md_.count_hit_[idx_ctns] = md_.count_hit_and_miss_[idx_ctns] = 0; + + if (log_odds_update >= 0 && md_.occupancy_buffer_[idx_ctns] >= mp_.clamp_max_log_) + { + continue; + } + else if (log_odds_update <= 0 && md_.occupancy_buffer_[idx_ctns] <= mp_.clamp_min_log_) + { + md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_; + continue; + } + + bool in_local = idx(0) >= min_id(0) && idx(0) <= max_id(0) && idx(1) >= min_id(1) && + idx(1) <= max_id(1) && idx(2) >= min_id(2) && idx(2) <= max_id(2); + if (!in_local) + { + md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_; + } + + md_.occupancy_buffer_[idx_ctns] = + std::min(std::max(md_.occupancy_buffer_[idx_ctns] + log_odds_update, mp_.clamp_min_log_), + mp_.clamp_max_log_); + } +} + +Eigen::Vector3d GridMap::closetPointInMap(const Eigen::Vector3d &pt, const Eigen::Vector3d &camera_pt) +{ + Eigen::Vector3d diff = pt - camera_pt; + Eigen::Vector3d max_tc = mp_.map_max_boundary_ - camera_pt; + Eigen::Vector3d min_tc = mp_.map_min_boundary_ - camera_pt; + + double min_t = 1000000; + + for (int i = 0; i < 3; ++i) + { + if (fabs(diff[i]) > 0) + { + + double t1 = max_tc[i] / diff[i]; + if (t1 > 0 && t1 < min_t) + min_t = t1; + + double t2 = min_tc[i] / diff[i]; + if (t2 > 0 && t2 < min_t) + min_t = t2; + } + } + + return camera_pt + (min_t - 1e-3) * diff; +} + +void GridMap::clearAndInflateLocalMap() +{ + /*clear outside local*/ + const int vec_margin = 5; + // Eigen::Vector3i min_vec_margin = min_vec - Eigen::Vector3i(vec_margin, + // vec_margin, vec_margin); Eigen::Vector3i max_vec_margin = max_vec + + // Eigen::Vector3i(vec_margin, vec_margin, vec_margin); + + Eigen::Vector3i min_cut = md_.local_bound_min_ - + Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_); + Eigen::Vector3i max_cut = md_.local_bound_max_ + + Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_); + boundIndex(min_cut); + boundIndex(max_cut); + + Eigen::Vector3i min_cut_m = min_cut - Eigen::Vector3i(vec_margin, vec_margin, vec_margin); + Eigen::Vector3i max_cut_m = max_cut + Eigen::Vector3i(vec_margin, vec_margin, vec_margin); + boundIndex(min_cut_m); + boundIndex(max_cut_m); + + // clear data outside the local range + + for (int x = min_cut_m(0); x <= max_cut_m(0); ++x) + for (int y = min_cut_m(1); y <= max_cut_m(1); ++y) + { + + for (int z = min_cut_m(2); z < min_cut(2); ++z) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + + for (int z = max_cut(2) + 1; z <= max_cut_m(2); ++z) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + } + + for (int z = min_cut_m(2); z <= max_cut_m(2); ++z) + for (int x = min_cut_m(0); x <= max_cut_m(0); ++x) + { + + for (int y = min_cut_m(1); y < min_cut(1); ++y) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + + for (int y = max_cut(1) + 1; y <= max_cut_m(1); ++y) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + } + + for (int y = min_cut_m(1); y <= max_cut_m(1); ++y) + for (int z = min_cut_m(2); z <= max_cut_m(2); ++z) + { + + for (int x = min_cut_m(0); x < min_cut(0); ++x) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + + for (int x = max_cut(0) + 1; x <= max_cut_m(0); ++x) + { + int idx = toAddress(x, y, z); + md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_; + } + } + + // inflate occupied voxels to compensate robot size + + int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_); + // int inf_step_z = 1; + vector inf_pts(pow(2 * inf_step + 1, 3)); + // inf_pts.resize(4 * inf_step + 3); + Eigen::Vector3i inf_pt; + + // clear outdated data + for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) + for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) + for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z) + { + md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0; + } + + // inflate obstacles + for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) + for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) + for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z) + { + + if (md_.occupancy_buffer_[toAddress(x, y, z)] > mp_.min_occupancy_log_) + { + inflatePoint(Eigen::Vector3i(x, y, z), inf_step, inf_pts); + + for (int k = 0; k < (int)inf_pts.size(); ++k) + { + inf_pt = inf_pts[k]; + int idx_inf = toAddress(inf_pt); + if (idx_inf < 0 || + idx_inf >= mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2)) + { + continue; + } + md_.occupancy_buffer_inflate_[idx_inf] = 1; + } + } + } + + // add virtual ceiling to limit flight height + if (mp_.virtual_ceil_height_ > -0.5) { + int ceil_id = floor((mp_.virtual_ceil_height_ - mp_.map_origin_(2)) * mp_.resolution_inv_); + for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) + for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) { + md_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1; + } + } +} + +void GridMap::visCallback(const ros::TimerEvent & /*event*/) +{ + + publishMapInflate(true); + publishMap(); +} + +void GridMap::updateOccupancyCallback(const ros::TimerEvent & /*event*/) +{ + if (md_.last_occ_update_time_.toSec() < 1.0 ) md_.last_occ_update_time_ = ros::Time::now(); + + if (!md_.occ_need_update_) + { + if ( md_.flag_use_depth_fusion && (ros::Time::now() - md_.last_occ_update_time_).toSec() > mp_.odom_depth_timeout_ ) + { + ROS_ERROR("odom or depth lost! ros::Time::now()=%f, md_.last_occ_update_time_=%f, mp_.odom_depth_timeout_=%f", + ros::Time::now().toSec(), md_.last_occ_update_time_.toSec(), mp_.odom_depth_timeout_); + md_.flag_depth_odom_timeout_ = true; + } + return; + } + md_.last_occ_update_time_ = ros::Time::now(); + + /* update occupancy */ + // ros::Time t1, t2, t3, t4; + // t1 = ros::Time::now(); + + projectDepthImage(); + // t2 = ros::Time::now(); + raycastProcess(); + // t3 = ros::Time::now(); + + if (md_.local_updated_) + clearAndInflateLocalMap(); + + // t4 = ros::Time::now(); + + // cout << setprecision(7); + // cout << "t2=" << (t2-t1).toSec() << " t3=" << (t3-t2).toSec() << " t4=" << (t4-t3).toSec() << endl;; + + // md_.fuse_time_ += (t2 - t1).toSec(); + // md_.max_fuse_time_ = max(md_.max_fuse_time_, (t2 - t1).toSec()); + + // if (mp_.show_occ_time_) + // ROS_WARN("Fusion: cur t = %lf, avg t = %lf, max t = %lf", (t2 - t1).toSec(), + // md_.fuse_time_ / md_.update_num_, md_.max_fuse_time_); + + md_.occ_need_update_ = false; + md_.local_updated_ = false; +} + +void GridMap::depthPoseCallback(const sensor_msgs::ImageConstPtr &img, + const geometry_msgs::PoseStampedConstPtr &pose) +{ + /* get depth image */ + cv_bridge::CvImagePtr cv_ptr; + cv_ptr = cv_bridge::toCvCopy(img, img->encoding); + + if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1) + { + (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_); + } + cv_ptr->image.copyTo(md_.depth_image_); + + // std::cout << "depth: " << md_.depth_image_.cols << ", " << md_.depth_image_.rows << std::endl; + + /* get pose */ + md_.camera_pos_(0) = pose->pose.position.x; + md_.camera_pos_(1) = pose->pose.position.y; + md_.camera_pos_(2) = pose->pose.position.z; + md_.camera_r_m_ = Eigen::Quaterniond(pose->pose.orientation.w, pose->pose.orientation.x, + pose->pose.orientation.y, pose->pose.orientation.z) + .toRotationMatrix(); + if (isInMap(md_.camera_pos_)) + { + md_.has_odom_ = true; + md_.update_num_ += 1; + md_.occ_need_update_ = true; + } + else + { + md_.occ_need_update_ = false; + } + + md_.flag_use_depth_fusion = true; +} + +void GridMap::odomCallback(const nav_msgs::OdometryConstPtr &odom) +{ + if (md_.has_first_depth_) + return; + + md_.camera_pos_(0) = odom->pose.pose.position.x; + md_.camera_pos_(1) = odom->pose.pose.position.y; + md_.camera_pos_(2) = odom->pose.pose.position.z; + + md_.has_odom_ = true; +} + +void GridMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &img) +{ + + pcl::PointCloud latest_cloud; + pcl::fromROSMsg(*img, latest_cloud); + + md_.has_cloud_ = true; + + if (!md_.has_odom_) + { + std::cout << "no odom!" << std::endl; + return; + } + + if (latest_cloud.points.size() == 0) + return; + + if (isnan(md_.camera_pos_(0)) || isnan(md_.camera_pos_(1)) || isnan(md_.camera_pos_(2))) + return; + + this->resetBuffer(md_.camera_pos_ - mp_.local_update_range_, + md_.camera_pos_ + mp_.local_update_range_); + + pcl::PointXYZ pt; + Eigen::Vector3d p3d, p3d_inf; + + int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_); + int inf_step_z = 1; + + double max_x, max_y, max_z, min_x, min_y, min_z; + + min_x = mp_.map_max_boundary_(0); + min_y = mp_.map_max_boundary_(1); + min_z = mp_.map_max_boundary_(2); + + max_x = mp_.map_min_boundary_(0); + max_y = mp_.map_min_boundary_(1); + max_z = mp_.map_min_boundary_(2); + + for (size_t i = 0; i < latest_cloud.points.size(); ++i) + { + pt = latest_cloud.points[i]; + p3d(0) = pt.x, p3d(1) = pt.y, p3d(2) = pt.z; + + /* point inside update range */ + Eigen::Vector3d devi = p3d - md_.camera_pos_; + Eigen::Vector3i inf_pt; + + if (fabs(devi(0)) < mp_.local_update_range_(0) && fabs(devi(1)) < mp_.local_update_range_(1) && + fabs(devi(2)) < mp_.local_update_range_(2)) + { + + /* inflate the point */ + for (int x = -inf_step; x <= inf_step; ++x) + for (int y = -inf_step; y <= inf_step; ++y) + for (int z = -inf_step_z; z <= inf_step_z; ++z) + { + + p3d_inf(0) = pt.x + x * mp_.resolution_; + p3d_inf(1) = pt.y + y * mp_.resolution_; + p3d_inf(2) = pt.z + z * mp_.resolution_; + + max_x = max(max_x, p3d_inf(0)); + max_y = max(max_y, p3d_inf(1)); + max_z = max(max_z, p3d_inf(2)); + + min_x = min(min_x, p3d_inf(0)); + min_y = min(min_y, p3d_inf(1)); + min_z = min(min_z, p3d_inf(2)); + + posToIndex(p3d_inf, inf_pt); + + if (!isInMap(inf_pt)) + continue; + + int idx_inf = toAddress(inf_pt); + + md_.occupancy_buffer_inflate_[idx_inf] = 1; + } + } + } + + min_x = min(min_x, md_.camera_pos_(0)); + min_y = min(min_y, md_.camera_pos_(1)); + min_z = min(min_z, md_.camera_pos_(2)); + + max_x = max(max_x, md_.camera_pos_(0)); + max_y = max(max_y, md_.camera_pos_(1)); + max_z = max(max_z, md_.camera_pos_(2)); + + max_z = max(max_z, mp_.ground_height_); + + posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_); + posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_); + + boundIndex(md_.local_bound_min_); + boundIndex(md_.local_bound_max_); + + // add virtual ceiling to limit flight height + if (mp_.virtual_ceil_height_ > -0.5) { + int ceil_id = floor((mp_.virtual_ceil_height_ - mp_.map_origin_(2)) * mp_.resolution_inv_); + for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x) + for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) { + md_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1; + } + } +} + +void GridMap::publishMap() +{ + + if (map_pub_.getNumSubscribers() <= 0) + return; + + pcl::PointXYZ pt; + pcl::PointCloud cloud; + + Eigen::Vector3i min_cut = md_.local_bound_min_; + Eigen::Vector3i max_cut = md_.local_bound_max_; + + int lmm = mp_.local_map_margin_ / 2; + min_cut -= Eigen::Vector3i(lmm, lmm, lmm); + max_cut += Eigen::Vector3i(lmm, lmm, lmm); + + boundIndex(min_cut); + boundIndex(max_cut); + + for (int x = min_cut(0); x <= max_cut(0); ++x) + for (int y = min_cut(1); y <= max_cut(1); ++y) + for (int z = min_cut(2); z <= max_cut(2); ++z) + { + if (md_.occupancy_buffer_[toAddress(x, y, z)] < mp_.min_occupancy_log_) + continue; + + Eigen::Vector3d pos; + indexToPos(Eigen::Vector3i(x, y, z), pos); + if (pos(2) > mp_.visualization_truncate_height_) + continue; + + pt.x = pos(0); + pt.y = pos(1); + pt.z = pos(2); + cloud.push_back(pt); + } + + cloud.width = cloud.points.size(); + cloud.height = 1; + cloud.is_dense = true; + cloud.header.frame_id = mp_.frame_id_; + sensor_msgs::PointCloud2 cloud_msg; + + pcl::toROSMsg(cloud, cloud_msg); + map_pub_.publish(cloud_msg); +} + +void GridMap::publishMapInflate(bool all_info) +{ + + if (map_inf_pub_.getNumSubscribers() <= 0) + return; + + pcl::PointXYZ pt; + pcl::PointCloud cloud; + + Eigen::Vector3i min_cut = md_.local_bound_min_; + Eigen::Vector3i max_cut = md_.local_bound_max_; + + if (all_info) + { + int lmm = mp_.local_map_margin_; + min_cut -= Eigen::Vector3i(lmm, lmm, lmm); + max_cut += Eigen::Vector3i(lmm, lmm, lmm); + } + + boundIndex(min_cut); + boundIndex(max_cut); + + for (int x = min_cut(0); x <= max_cut(0); ++x) + for (int y = min_cut(1); y <= max_cut(1); ++y) + for (int z = min_cut(2); z <= max_cut(2); ++z) + { + if (md_.occupancy_buffer_inflate_[toAddress(x, y, z)] == 0) + continue; + + Eigen::Vector3d pos; + indexToPos(Eigen::Vector3i(x, y, z), pos); + if (pos(2) > mp_.visualization_truncate_height_) + continue; + + pt.x = pos(0); + pt.y = pos(1); + pt.z = pos(2); + cloud.push_back(pt); + } + + cloud.width = cloud.points.size(); + cloud.height = 1; + cloud.is_dense = true; + cloud.header.frame_id = mp_.frame_id_; + sensor_msgs::PointCloud2 cloud_msg; + + pcl::toROSMsg(cloud, cloud_msg); + map_inf_pub_.publish(cloud_msg); + + // ROS_INFO("pub map"); +} + +bool GridMap::odomValid() { return md_.has_odom_; } + +bool GridMap::hasDepthObservation() { return md_.has_first_depth_; } + +Eigen::Vector3d GridMap::getOrigin() { return mp_.map_origin_; } + +// int GridMap::getVoxelNum() { +// return mp_.map_voxel_num_[0] * mp_.map_voxel_num_[1] * mp_.map_voxel_num_[2]; +// } + +void GridMap::getRegion(Eigen::Vector3d &ori, Eigen::Vector3d &size) +{ + ori = mp_.map_origin_, size = mp_.map_size_; +} + +void GridMap::extrinsicCallback(const nav_msgs::OdometryConstPtr &odom) +{ + Eigen::Quaterniond cam2body_q = Eigen::Quaterniond(odom->pose.pose.orientation.w, + odom->pose.pose.orientation.x, + odom->pose.pose.orientation.y, + odom->pose.pose.orientation.z); + Eigen::Matrix3d cam2body_r_m = cam2body_q.toRotationMatrix(); + md_.cam2body_.block<3, 3>(0, 0) = cam2body_r_m; + md_.cam2body_(0, 3) = odom->pose.pose.position.x; + md_.cam2body_(1, 3) = odom->pose.pose.position.y; + md_.cam2body_(2, 3) = odom->pose.pose.position.z; + md_.cam2body_(3, 3) = 1.0; +} + +void GridMap::depthOdomCallback(const sensor_msgs::ImageConstPtr &img, + const nav_msgs::OdometryConstPtr &odom) +{ + /* get pose */ + Eigen::Quaterniond body_q = Eigen::Quaterniond(odom->pose.pose.orientation.w, + odom->pose.pose.orientation.x, + odom->pose.pose.orientation.y, + odom->pose.pose.orientation.z); + Eigen::Matrix3d body_r_m = body_q.toRotationMatrix(); + Eigen::Matrix4d body2world; + body2world.block<3, 3>(0, 0) = body_r_m; + body2world(0, 3) = odom->pose.pose.position.x; + body2world(1, 3) = odom->pose.pose.position.y; + body2world(2, 3) = odom->pose.pose.position.z; + body2world(3, 3) = 1.0; + + Eigen::Matrix4d cam_T = body2world * md_.cam2body_; + md_.camera_pos_(0) = cam_T(0, 3); + md_.camera_pos_(1) = cam_T(1, 3); + md_.camera_pos_(2) = cam_T(2, 3); + md_.camera_r_m_ = cam_T.block<3, 3>(0, 0); + + /* get depth image */ + cv_bridge::CvImagePtr cv_ptr; + cv_ptr = cv_bridge::toCvCopy(img, img->encoding); + if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1) + { + (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_); + } + cv_ptr->image.copyTo(md_.depth_image_); + + md_.occ_need_update_ = true; + md_.flag_use_depth_fusion = true; +} diff --git a/motion_planning/3d/ego_planner/planner/plan_env/src/obj_generator.cpp b/motion_planning/3d/ego_planner/planner/plan_env/src/obj_generator.cpp new file mode 100644 index 0000000..20b0701 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_env/src/obj_generator.cpp @@ -0,0 +1,238 @@ +/** +* This file is part of Fast-Planner. +* +* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, +* Developed by Boyu Zhou , +* for more information see . +* 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 . +*/ + + + +#include "visualization_msgs/Marker.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +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 pose_pubs; // obj pose (from optitrack) +vector obj_models; + +random_device rd; +default_random_engine eng(rd()); +uniform_real_distribution rand_pos_x; +uniform_real_distribution rand_pos_y; +uniform_real_distribution rand_h; +uniform_real_distribution rand_vel; +uniform_real_distribution rand_acc_r; +uniform_real_distribution rand_acc_t; +uniform_real_distribution rand_acc_z; +uniform_real_distribution rand_color; +uniform_real_distribution rand_scale; +uniform_real_distribution rand_yaw_dot; +uniform_real_distribution 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("/dynamic/obj", 10); + for (int i = 0; i < obj_num; ++i) { + ros::Publisher pose_pub = + node.advertise("/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(0.0, 1.0); + rand_pos_x = uniform_real_distribution(-_x_size/2, _x_size/2); + rand_pos_y = uniform_real_distribution(-_y_size/2, _y_size/2); + rand_h = uniform_real_distribution(0.0, _h_size); + rand_vel = uniform_real_distribution(-_vel, _vel); + rand_acc_t = uniform_real_distribution(0.0, 6.28); + rand_acc_r = uniform_real_distribution(_acc_r1, _acc_r2); + rand_acc_z = uniform_real_distribution(-_acc_z, _acc_z); + rand_scale = uniform_real_distribution(_scale1, _scale2); + rand_yaw = uniform_real_distribution(0.0, 2 * 3.141592); + rand_yaw_dot = uniform_real_distribution(-_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); +} diff --git a/motion_planning/3d/ego_planner/planner/plan_env/src/obj_predictor.cpp b/motion_planning/3d/ego_planner/planner/plan_env/src/obj_predictor.cpp new file mode 100644 index 0000000..d960cda --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_env/src/obj_predictor.cpp @@ -0,0 +1,289 @@ +/** +* This file is part of Fast-Planner. +* +* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, +* Developed by Boyu Zhou , +* for more information see . +* 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 . +*/ + + + +#include +#include + +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); + predict_trajs_->resize(obj_num_); + + obj_scale_.reset(new vector); + 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 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( + "/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("/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 A; + Eigen::Matrix temp; + Eigen::Matrix bm[3]; // poly coefficent + vector> pm(3); + + A.setZero(); + for (int i = 0; i < 3; ++i) + bm[i].setZero(); + + /* ---------- estimation error ---------- */ + list his; + obj_histories_[i]->getHistory(his); + for (list::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 his; + obj_histories_[i]->getHistory(his); + // if ( i==0 ) + // { + // cout << "his.size()=" << his.size() << endl; + // for ( auto hi:his ) + // { + // cout << hi.transpose() << endl; + // } + // } + list::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 p01, q12; + q12.row(0) = q1.transpose(); + q12.row(1) = q2.transpose(); + + Eigen::Matrix At12; + At12 << 1, t1, 1, t2; + + p01 = At12.inverse() * q12; + + vector> 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::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::max(); + return Eigen::Vector3d(MAX, MAX, MAX); +} + +// ObjPredictor:: +} // namespace fast_planner \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/plan_env/src/raycast.cpp b/motion_planning/3d/ego_planner/planner/plan_env/src/raycast.cpp new file mode 100644 index 0000000..56ffdbf --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_env/src/raycast.cpp @@ -0,0 +1,321 @@ +#include +#include +#include +#include + +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 + // + // + // 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* output) { + // std::cout << start << ' ' << end << std::endl; + // From "A Fast Voxel Traversal Algorithm for Ray Tracing" + // by John Amanatides and Andrew Woo, 1987 + // + // + // 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; +} \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/CMakeLists.txt b/motion_planning/3d/ego_planner/planner/plan_manage/CMakeLists.txt new file mode 100755 index 0000000..aa7c97e --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/CMakeLists.txt @@ -0,0 +1,56 @@ +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 +) + +# 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}) + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/include/plan_manage/ego_replan_fsm.h b/motion_planning/3d/ego_planner/planner/plan_manage/include/plan_manage/ego_replan_fsm.h new file mode 100644 index 0000000..87f1281 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/include/plan_manage/ego_replan_fsm.h @@ -0,0 +1,130 @@ +#ifndef _REBO_REPLAN_FSM_H_ +#define _REBO_REPLAN_FSM_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +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 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 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 \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/include/plan_manage/planner_manager.h b/motion_planning/3d/ego_planner/planner/plan_manage/include/plan_manage/planner_manager.h new file mode 100644 index 0000000..ce54642 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/include/plan_manage/planner_manager.h @@ -0,0 +1,85 @@ +#ifndef _PLANNER_MANAGER_H_ +#define _PLANNER_MANAGER_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +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 &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 &start_end_derivative, double ratio, Eigen::MatrixXd &ctrl_pts, double &dt, + double &time_inc); + + bool refineTrajAlgo(UniformBspline &traj, vector &start_end_derivative, double ratio, double &ts, Eigen::MatrixXd &optimal_control_points); + + // !SECTION stable + + // SECTION developing + + public: + typedef unique_ptr Ptr; + + // !SECTION + }; +} // namespace ego_planner + +#endif \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/launch/advanced_param.xml b/motion_planning/3d/ego_planner/planner/plan_manage/launch/advanced_param.xml new file mode 100644 index 0000000..769467c --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/launch/advanced_param.xml @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/launch/advanced_param_xtdrone.xml b/motion_planning/3d/ego_planner/planner/plan_manage/launch/advanced_param_xtdrone.xml new file mode 100644 index 0000000..90a4a33 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/launch/advanced_param_xtdrone.xml @@ -0,0 +1,160 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/launch/default.rviz b/motion_planning/3d/ego_planner/planner/plan_manage/launch/default.rviz new file mode 100644 index 0000000..b095392 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/launch/default.rviz @@ -0,0 +1,3028 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /simulation_map1/Autocompute Value Bounds1 + - /drone01/Planning1 + - /drone01/Planning1/drone_path1 + - /drone01/Planning1/drone_path1/Offset1 + - /drone01/Mapping1 + - /drone01/Mapping1/map inflate1 + - /drone11/Planning1 + - /drone11/Planning1/drone_path1 + - /drone11/Mapping1 + - /drone11/Mapping1/map inflate1 + - /drone11/Simulation1 + - /drone21/Planning1 + - /drone21/Planning1/drone_path1 + - /drone21/Mapping1 + - /drone21/Mapping1/map inflate1 + - /drone31/Planning1 + - /drone31/Planning1/drone_path1 + - /drone31/Mapping1 + - /drone31/Mapping1/map inflate1 + - /drone41/Planning1 + - /drone41/Planning1/optimal_traj1 + - /drone41/Planning1/drone_path1 + - /drone41/Mapping1 + - /drone41/Mapping1/map inflate1 + - /drone51/Planning1 + - /drone51/Planning1/drone_path1 + - /drone51/Mapping1 + - /drone51/Mapping1/map inflate1 + - /drone61/Planning1 + - /drone61/Planning1/drone_path1 + - /drone61/Mapping1 + - /drone61/Mapping1/map inflate1 + - /drone71/Planning1 + - /drone71/Planning1/InitTraj1 + - /drone71/Planning1/drone_path1 + - /drone71/Mapping1 + - /drone71/Mapping1/map inflate1 + - /drone71/Simulation1 + - /drone71/Simulation1/robot1 + - /drone81/Planning1 + - /drone81/Planning1/drone_path1 + - /drone81/Mapping1 + - /drone81/Mapping1/map inflate1 + - /drone81/Simulation1/robot1 + - /drone91/Planning1 + - /drone91/Planning1/drone_path1 + - /drone91/Mapping1 + - /drone91/Mapping1/map inflate1 + - /drone91/Simulation1 + - /drone91/Simulation1/robot1 + - /drone101/Planning1 + - /drone101/Planning1/optimal_traj1 + - /drone101/Planning1/drone_path1 + - /drone101/Simulation1 + - /drone101/Simulation1/robot1 + - /drone111/Planning1 + - /drone111/Planning1/optimal_traj1 + - /drone111/Simulation1 + - /drone111/Simulation1/robot1 + - /drone121/Planning1 + - /drone121/Planning1/optimal_traj1 + - /drone121/Planning1/drone_path1 + - /drone121/Simulation1 + - /drone121/Simulation1/robot1 + - /drone131/Planning1 + - /drone131/Planning1/optimal_traj1 + - /drone131/Planning1/drone_path1 + - /drone131/Simulation1/robot1 + - /drone141/Planning1 + - /drone141/Planning1/drone_path1 + - /drone141/Simulation1/robot1 + - /drone151/Planning1 + - /drone151/Planning1/optimal_traj1 + - /drone151/Planning1/drone_path1 + - /drone151/Simulation1 + - /drone151/Simulation1/robot1 + - /drone161/Planning1 + - /drone161/Planning1/optimal_traj1 + - /drone161/Planning1/drone_path1 + - /drone161/Simulation1 + - /drone161/Simulation1/robot1 + - /drone171/Planning1 + - /drone171/Planning1/optimal_traj1 + - /drone171/Planning1/drone_path1 + - /drone171/Simulation1 + - /drone171/Simulation1/robot1 + - /drone181/Planning1 + - /drone181/Planning1/optimal_traj1 + - /drone181/Planning1/drone_path1 + - /drone181/Simulation1 + - /drone181/Simulation1/robot1 + - /drone191/Planning1 + - /drone191/Planning1/goal_point1 + - /drone191/Planning1/optimal_traj1 + - /drone191/Planning1/drone_path1 + - /drone191/Simulation1 + - /drone191/Simulation1/robot1 + - /drone201/Planning1 + - /drone201/Planning1/drone_path1 + - /drone201/Simulation1 + - /drone201/Simulation1/robot1 + Splitter Ratio: 0.436111122 + Tree Height: 728 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: simulation_map +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Class: rviz/Axes + Enabled: false + Length: 1 + Name: Axes + Radius: 0.100000001 + Reference Frame: + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 1000 + Reference Frame: + Value: true + - Alpha: 0.150000006 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.9000001 + Min Value: 0 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 85; 170; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: simulation_map + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Boxes + Topic: /map_generator/global_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_0_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_0_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_0_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 29; 108; 212 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_0_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.83999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 29; 108; 212 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_0_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_0_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_0_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone0 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_1_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_1_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_1_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 170; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_1_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_1_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_1_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_1_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone1 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_2_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_2_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_2_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 170; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_2_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 170; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_2_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_2_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_2_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone2 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_3_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_3_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_3_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 0; 255 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_3_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 0; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_3_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_3_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_3_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone3 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_4_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_4_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_4_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_4_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 170; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_4_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_4_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_4_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone4 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_5_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_5_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_5_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_5_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_5_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_5_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone5 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_6_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_6_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_6_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 190; 120; 121 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_6_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 190; 120; 121 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_6_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_6_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_6_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone6 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_7_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_7_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_7_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 255 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_7_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 170; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_5_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_7_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone7 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_8_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_8_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_8_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 226; 0; 158 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_8_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 226; 0; 158 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_8_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_8_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone8 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_9_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + "": true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 85; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_9_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_9_odom_visualization/robot + Name: robot + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone9 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_10_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 29; 108; 212 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_10_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_10_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone10 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_11_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 170; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_11_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_11_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone11 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_12_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 170; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_12_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_12_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone12 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_13_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 0; 255 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_13_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_13_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone13 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_14_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_14_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_14_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone14 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_15_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_15_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_15_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone15 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_16_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 190; 120; 121 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_16_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_16_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone16 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_17_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 170; 255 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_17_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_17_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone17 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_18_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 226; 0; 158 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_18_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_18_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone18 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_19_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_19_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 85; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_19_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_19_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone19 + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/goal_point + Name: goal_point + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/global_list + Name: global_path + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_20_ego_planner_node/optimal_list + Name: optimal_traj + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /ego_planner_node/a_star_list + Name: AStar + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /drone_9_ego_planner_node/init_list + Name: InitTraj + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 20; 106; 172 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Billboards + Line Width: 0.100000001 + Name: drone_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /drone_20_odom_visualization/path + Unreliable: false + Value: true + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.33999991 + Min Value: 0.0399999991 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 170; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: map inflate + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /drone_9_ego_planner_node/grid_map/occupancy_inflate + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Mapping + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /drone_20_odom_visualization/robot + Name: robot + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /drone_5_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: depth + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Simulation + Enabled: true + Name: drone20 + - Class: rviz/Image + Enabled: false + Image Topic: /drone_0_pcl_render_node/depth + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 50.3524513 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -10.8770046 + Y: -0.202289343 + Z: 9.53674316e-07 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 1.56979632 + Target Frame: + Value: ThirdPersonFollower (rviz) + Yaw: 3.1415925 + Saved: + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.00999999978 + Pitch: 0.400000006 + Position: + X: -11 + Y: 0 + Z: 8 + Target Frame: my_view + Value: FPS (rviz) + Yaw: 0 + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.00999999978 + Pitch: 0.5 + Position: + X: -10 + Y: 0 + Z: 10 + Target Frame: my_view + Value: FPS (rviz) + Yaw: 0 + - Class: rviz/FPS + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: FPS + Near Clip Distance: 0.00999999978 + Pitch: 0.600000024 + Position: + X: -10 + Y: 0 + Z: 10 + Target Frame: my_view + Value: FPS (rviz) + Yaw: 0 +Window Geometry: + Displays: + collapsed: true + Height: 1388 + Hide Left Dock: true + Hide Right Dock: false + Image: + collapsed: true + QMainWindow State: 000000ff00000000fd00000004000000000000019200000526fc0200000024fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000319000000d700fffffffb0000000a005600690065007700730000000347000000ad000000ad00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000001d1000000b50000000000000000fb0000000a0049006d00610067006502000001a2000001e1000000f8000000b5fb0000000a00640065007000740068000000031d000002310000001600fffffffb0000000a0049006d0061006700650100000415000000f80000000000000000fb0000000a0049006d00610067006501000003f4000001190000000000000000fb0000000a0064006500700074006800000004a2000000ac0000001600fffffffb0000000a0064006500700074006800000003b4000001b60000001600fffffffb0000000a0064006500700074006800000004b9000000950000001600fffffffb0000000a006400650070007400680000000481000000cd0000001600fffffffb0000000a006400650070007400680000000429000001250000001600fffffffb0000000a0064006500700074006800000003b7000000af0000001600fffffffb0000000a0064006500700074006800000003d5000000b50000001600fffffffb0000000a006400650070007400680000000444000001260000001600fffffffb0000000a00640065007000740068000000041f0000014b0000001600fffffffb0000000a0064006500700074006800000001b8000000160000001600fffffffb0000000a0064006500700074006800000001c1000000160000001600fffffffb0000000a0064006500700074006800000004b1000000550000001600fffffffb0000000a006400650070007400680000000505000000650000001600fffffffb0000000a006400650070007400680000000502000000680000001600fffffffb0000000a0064006500700074006800000004f9000000710000001600fffffffb0000000a0064006500700074006800000004e9000000810000001600fffffffb0000000a0064006500700074006800000004de0000008c0000001600fffffffb0000000a0064006500700074006800000004cc0000009e0000001600fffffffb0000000a0064006500700074006800000004bb000000af0000001600fffffffb0000000a0064006500700074006800000004aa000000c00000001600fffffffb0000000a006400650070007400680000000498000000d20000000000000000fb0000000a0049006d00610067006500000003fa000001540000001600ffffff000000010000010f00000385fc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006100000003bfc0100000002fb0000000800540069006d00650000000000000006100000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005000000052600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1280 + X: 3840 + Y: 24 + depth: + collapsed: false diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/launch/run_in_sim.launch b/motion_planning/3d/ego_planner/planner/plan_manage/launch/run_in_sim.launch new file mode 100644 index 0000000..0eb4ad7 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/launch/run_in_sim.launch @@ -0,0 +1,135 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + > + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/launch/run_in_xtdrone.launch b/motion_planning/3d/ego_planner/planner/plan_manage/launch/run_in_xtdrone.launch new file mode 100644 index 0000000..18a4e3a --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/launch/run_in_xtdrone.launch @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/launch/rviz.launch b/motion_planning/3d/ego_planner/planner/plan_manage/launch/rviz.launch new file mode 100644 index 0000000..667873d --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/launch/rviz.launch @@ -0,0 +1,3 @@ + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/launch/simple_run.launch b/motion_planning/3d/ego_planner/planner/plan_manage/launch/simple_run.launch new file mode 100644 index 0000000..0e5e034 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/launch/simple_run.launch @@ -0,0 +1,4 @@ + + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/launch/simulator.xml b/motion_planning/3d/ego_planner/planner/plan_manage/launch/simulator.xml new file mode 100755 index 0000000..30800f5 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/launch/simulator.xml @@ -0,0 +1,141 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/launch/single_uav.launch b/motion_planning/3d/ego_planner/planner/plan_manage/launch/single_uav.launch new file mode 100644 index 0000000..fff0d06 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/launch/single_uav.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/launch/swarm.launch b/motion_planning/3d/ego_planner/planner/plan_manage/launch/swarm.launch new file mode 100644 index 0000000..1cc2db5 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/launch/swarm.launch @@ -0,0 +1,205 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/launch/swarm_large.launch b/motion_planning/3d/ego_planner/planner/plan_manage/launch/swarm_large.launch new file mode 100644 index 0000000..d3794d1 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/launch/swarm_large.launch @@ -0,0 +1,412 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/package.xml b/motion_planning/3d/ego_planner/planner/plan_manage/package.xml new file mode 100755 index 0000000..c3776ea --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/package.xml @@ -0,0 +1,82 @@ + + + ego_planner + 0.0.0 + The ego_planner package + + + + + iszhouxin + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + + plan_env + path_searching + bspline_opt + traj_utils + + roscpp + std_msgs + + roscpp + std_msgs + message_runtime + + plan_env + path_searching + bspline_opt + traj_utils + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/src/ego_planner_node.cpp b/motion_planning/3d/ego_planner/planner/plan_manage/src/ego_planner_node.cpp new file mode 100644 index 0000000..21ef6c3 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/src/ego_planner_node.cpp @@ -0,0 +1,56 @@ +#include +#include + +#include + +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 +// #include +// #include + +// #include + +// 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; +// } \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/src/ego_replan_fsm.cpp b/motion_planning/3d/ego_planner/planner/plan_manage/src/ego_replan_fsm.cpp new file mode 100644 index 0000000..3bf8c10 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/src/ego_replan_fsm.cpp @@ -0,0 +1,948 @@ + +#include + +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 */ + 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/planning_horizen_time", planning_horizen_time_, -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); + + have_trigger_ = !flag_realworld_experiment_; + + 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); + + if (planner_manager_->pp_.drone_id >= 1) + { + string sub_topic_name = string("/drone_") + 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("/drone_") + std::to_string(planner_manager_->pp_.drone_id) + string("_planning/swarm_trajs"); + swarm_trajs_pub_ = nh.advertise(pub_topic_name.c_str(), 10); + + broadcast_bspline_pub_ = nh.advertise("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("planning/bspline", 10); + data_disp_pub_ = nh.advertise("planning/data_display", 100); + + if (target_type_ == TARGET_TYPE::MANUAL_TARGET) + { + waypoint_sub_ = nh.subscribe("/move_base_simple/goal", 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]; + + // end_pt_ = wps_.back(); + } + + // bool success = planner_manager_->planGlobalTrajWaypoints( + // odom_pos_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), + // wps_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + + for (size_t i = 0; i < (size_t)waypoint_num_; i++) + { + visualization_->displayGoalPoint(wps_[i], Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, i); + ros::Duration(0.001).sleep(); + } + + // plan first global waypoint + wp_id_ = 0; + planNextWaypoint(wps_[wp_id_]); + + // if (success) + // { + + // /*** display ***/ + // constexpr double step_size_t = 0.1; + // int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t); + // std::vector gloabl_traj(i_end); + // for (int i = 0; i < i_end; i++) + // { + // 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"); + // // trigger_ = true; + // // else if (exec_state_ == EXEC_TRAJ) + // // changeFSMExecState(REPLAN_TRAJ, "TRIG"); + + // // visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(1, 0, 0, 1), 0.3, 0); + // ros::Duration(0.001).sleep(); + // visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0); + // ros::Duration(0.001).sleep(); + // } + // else + // { + // ROS_ERROR("Unable to generate global trajectory!"); + // } + } + + void EGOReplanFSM::planNextWaypoint(const Eigen::Vector3d next_wp) + { + bool success = false; + success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), next_wp, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + + // visualization_->displayGoalPoint(next_wp, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0); + + if (success) + { + end_pt_ = next_wp; + + /*** display ***/ + constexpr double step_size_t = 0.1; + int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t); + vector gloabl_traj(i_end); + for (int i = 0; i < i_end; i++) + { + 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"); + } + + // visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(1, 0, 0, 1), 0.3, 0); + 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; + + cout << "Triggered!" << endl; + // trigger_ = true; + init_pt_ = odom_pos_; + + Eigen::Vector3d end_wp(msg->pose.position.x, msg->pose.position.y, 1.0); + + 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) + { + 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 */ + 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", 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; + } + + 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; + cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl; + } + + std::pair EGOReplanFSM::timesOfConsecutiveStateCalls() + { + return std::pair(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 << "[FSM]: 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 == 100) + { + 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 + { + // if ( planner_manager_->pp_.drone_id <= 0 ) + // { + // changeFSMExecState(GEN_NEW_TRAJ, "FSM"); + // } + // else + // { + changeFSMExecState(SEQUENTIAL_START, "FSM"); + // } + } + break; + } + + case SEQUENTIAL_START: // for swarm + { + // cout << "id=" << planner_manager_->pp_.drone_id << " have_recv_pre_agent_=" << have_recv_pre_agent_ << endl; + if (planner_manager_->pp_.drone_id <= 0 || (planner_manager_->pp_.drone_id >= 1 && 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); + + 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) // close to the global target + { + 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; + 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_ = 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"); + } + + /* ---------- 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) + { + + 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; + + 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 */ + 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", 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_) + // { + // // todo + // ROS_ERROR("last_progress_time_ ERROR, TODO!"); + // ROS_ERROR("last_progress_time_ ERROR, TODO!"); + // ROS_ERROR("last_progress_time_ ERROR, TODO!"); + // ROS_ERROR("last_progress_time_ ERROR, TODO!"); + // ROS_ERROR("last_progress_time_ ERROR, TODO!"); + // cout << "dist=" << dist << endl; + // cout << "planner_manager_->global_data_.last_progress_time_=" << planner_manager_->global_data_.last_progress_time_ << endl; + // return; + // } + 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 + { + // planner_manager_->grid_map_; + local_target_pt_ = end_pt_; + } + + 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_ = (end_pt_ - init_pt_).normalized() * planner_manager_->pp_.max_vel_ * (( end_pt_ - local_target_pt_ ).norm() / ((planner_manager_->pp_.max_vel_*planner_manager_->pp_.max_vel_)/(2*planner_manager_->pp_.max_acc_))); + // cout << "A" << endl; + local_target_vel_ = Eigen::Vector3d::Zero(); + } + else + { + local_target_vel_ = planner_manager_->global_data_.getVelocity(t); + // cout << "AA" << endl; + } + } + +} // namespace ego_planner diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/src/planner_manager.cpp b/motion_planning/3d/ego_planner/planner/plan_manage/src/planner_manager.cpp new file mode 100644 index 0000000..a193620 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/src/planner_manager.cpp @@ -0,0 +1,567 @@ +// #include +#include +#include +#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); + nh.param("manager/drone_id", pp_.drone_id, -1); + + 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("/dynamic/obj_prdi", 10); // zx-todo + + 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; + printf("\033[47;30m\n[drone %d replan %d]==============================================\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 << "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 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 pseudo_arc_length; + vector 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> 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> vis_trajs; + + if (pp_.use_distinctive_trajs) + { + // cout << "enter" << endl; + std::vector trajs = bspline_optimizer_->distinctiveTrajs(segments); + 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)) + { + + 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; + + 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); + } + + cout << "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 ***/ + static bool print_once = true; + if ( print_once ) + { + print_once = false; + ROS_ERROR("IN SWARM MODE, REFINE DISABLED!"); + } + // disable refine in swarm scenario + // 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; + // } + + 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++; + 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; tgetSwarmClearance() ) + { + 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 &waypoints, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc) + { + + // generate global reference trajectory + + vector 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 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= 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 points; + points.push_back(start_pos); + points.push_back(end_pos); + + // insert intermediate points if too far + vector inter_points; + const double dist_thresh = 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()); + + // 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, end_pos, 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::refineTrajAlgo(UniformBspline &traj, vector &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 &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 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 diff --git a/motion_planning/3d/ego_planner/planner/plan_manage/src/traj_server.cpp b/motion_planning/3d/ego_planner/planner/plan_manage/src/traj_server.cpp new file mode 100644 index 0000000..9f97e74 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/plan_manage/src/traj_server.cpp @@ -0,0 +1,279 @@ +#include "bspline_opt/uniform_bspline.h" +#include "nav_msgs/Odometry.h" +#include "traj_utils/Bspline.h" +#include "quadrotor_msgs/PositionCommand.h" +#include "geometry_msgs/Pose.h" +#include "std_msgs/Empty.h" +#include "visualization_msgs/Marker.h" +#include + +ros::Publisher pos_cmd_pub; +ros::Publisher pose_cmd_pub; + +quadrotor_msgs::PositionCommand cmd; +geometry_msgs::Pose pose_cmd; +double pos_gain[3] = {0, 0, 0}; +double vel_gain[3] = {0, 0, 0}; + +using ego_planner::UniformBspline; + +bool receive_traj_ = false; +vector 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 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 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 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); + + pose_cmd.position.x = pos(0); + pose_cmd.position.y = pos(1); + pose_cmd.position.z = pos(2); + + pose_cmd.orientation.x = 0.0; + pose_cmd.orientation.y = 0.0; + pose_cmd.orientation.z = sin(yaw_yawdot.first/2); + pose_cmd.orientation.w = cos(yaw_yawdot.first/2); + + pose_cmd_pub.publish(pose_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("/position_cmd", 50); + pose_cmd_pub = nh.advertise("/pose_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); + last_yaw_ = 0.0; + last_yaw_dot_ = 0.0; + + ros::Duration(1.0).sleep(); + + ROS_WARN("[Traj server]: ready."); + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/CMakeLists.txt b/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/CMakeLists.txt new file mode 100755 index 0000000..30ec419 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/CMakeLists.txt @@ -0,0 +1,35 @@ +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} + ) + + diff --git a/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/launch/.bridge.launch.swp b/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/launch/.bridge.launch.swp new file mode 100644 index 0000000..e4cbf81 Binary files /dev/null and b/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/launch/.bridge.launch.swp differ diff --git a/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/launch/bridge.launch b/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/launch/bridge.launch new file mode 100644 index 0000000..babaa01 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/launch/bridge.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/package.xml b/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/package.xml new file mode 100755 index 0000000..677652f --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/package.xml @@ -0,0 +1,67 @@ + + + rosmsg_tcp_bridge + 0.0.0 + The rosmsg_tcp_bridge package + + + + + iszhouxin + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + traj_utils + + roscpp + std_msgs + traj_utils + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/src/bridge_node.cpp b/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/src/bridge_node.cpp new file mode 100644 index 0000000..71d3c79 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/rosmsg_tcp_bridge/src/bridge_node.cpp @@ -0,0 +1,828 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#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("/drone_") + 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_ >= 1 ) + { + string pub_traj_topic_name = string("/drone_") + std::to_string(drone_id_ - 1) + string("_planning/swarm_trajs"); + swarm_trajs_pub_ = nh.advertise(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("/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("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("/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; +} diff --git a/motion_planning/3d/ego_planner/planner/traj_utils/CMakeLists.txt b/motion_planning/3d/ego_planner/planner/traj_utils/CMakeLists.txt new file mode 100755 index 0000000..5f1f457 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/traj_utils/CMakeLists.txt @@ -0,0 +1,59 @@ +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} + ) diff --git a/motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/plan_container.hpp b/motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/plan_container.hpp new file mode 100644 index 0000000..4c249df --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/plan_container.hpp @@ -0,0 +1,233 @@ +#ifndef _PLAN_CONTAINER_H_ +#define _PLAN_CONTAINER_H_ + +#include +#include +#include + +#include +#include + +using std::vector; + +namespace ego_planner +{ + + class GlobalTrajData + { + private: + public: + PolynomialTraj global_traj_; + vector 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 &point_set, vector &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 &point_set, + vector &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 SwarmTrajData; + +} // namespace ego_planner + +#endif \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/planning_visualization.h b/motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/planning_visualization.h new file mode 100644 index 0000000..06d755b --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/planning_visualization.h @@ -0,0 +1,55 @@ +#ifndef _PLANNING_VISUALIZATION_H_ +#define _PLANNING_VISUALIZATION_H_ + +#include +#include +//#include +#include +//#include +#include +#include +#include +#include +#include + +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 Ptr; + + void displayMarkerList(ros::Publisher &pub, const vector &list, double scale, + Eigen::Vector4d color, int id, bool show_sphere = true); + void generatePathDisplayArray(visualization_msgs::MarkerArray &array, + const vector &list, double scale, Eigen::Vector4d color, int id); + void generateArrowDisplayArray(visualization_msgs::MarkerArray &array, + const vector &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 global_pts, const double scale, int id); + void displayInitPathList(vector init_pts, const double scale, int id); + void displayMultiInitPathList(vector> init_trajs, const double scale); + void displayOptimalList(Eigen::MatrixXd optimal_pts, int id); + void displayAStarList(std::vector> a_star_paths, int id); + void displayArrowList(ros::Publisher &pub, const vector &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 \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/polynomial_traj.h b/motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/polynomial_traj.h new file mode 100644 index 0000000..72bac9b --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/traj_utils/include/traj_utils/polynomial_traj.h @@ -0,0 +1,336 @@ +#ifndef _POLYNOMIAL_TRAJ_H +#define _POLYNOMIAL_TRAJ_H + +#include +#include + +using std::vector; + +class PolynomialTraj +{ +private: + vector times; // time of each segment + vector> cxs; // coefficient of x of each segment, from n-1 -> 0 + vector> cys; // coefficient of y of each segment + vector> czs; // coefficient of z of each segment + + double time_sum; + int num_seg; + + /* evaluation */ + vector 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 cx, vector cy, vector 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 getTimes() + { + return times; + } + + vector> 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> 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 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 \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/traj_utils/msg/Bspline.msg b/motion_planning/3d/ego_planner/planner/traj_utils/msg/Bspline.msg new file mode 100755 index 0000000..df4cfd4 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/traj_utils/msg/Bspline.msg @@ -0,0 +1,12 @@ +int32 drone_id + +int32 order +int64 traj_id +time start_time + +float64[] knots +geometry_msgs/Point[] pos_pts + +float64[] yaw_pts +float64 yaw_dt + diff --git a/motion_planning/3d/ego_planner/planner/traj_utils/msg/DataDisp.msg b/motion_planning/3d/ego_planner/planner/traj_utils/msg/DataDisp.msg new file mode 100644 index 0000000..742f28a --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/traj_utils/msg/DataDisp.msg @@ -0,0 +1,7 @@ +Header header + +float64 a +float64 b +float64 c +float64 d +float64 e \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/traj_utils/msg/MultiBsplines.msg b/motion_planning/3d/ego_planner/planner/traj_utils/msg/MultiBsplines.msg new file mode 100644 index 0000000..2f116f2 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/traj_utils/msg/MultiBsplines.msg @@ -0,0 +1,4 @@ +int32 drone_id_from + +Bspline[] traj + diff --git a/motion_planning/3d/ego_planner/planner/traj_utils/package.xml b/motion_planning/3d/ego_planner/planner/traj_utils/package.xml new file mode 100755 index 0000000..481ac26 --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/traj_utils/package.xml @@ -0,0 +1,77 @@ + + + traj_utils + 0.0.0 + The traj_utils package + + + + + bzhouai + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + roscpp + std_msgs + message_generation + + catkin + + + + std_msgs + + catkin + + + roscpp + std_msgs + message_runtime + + + + + + + + diff --git a/motion_planning/3d/ego_planner/planner/traj_utils/src/planning_visualization.cpp b/motion_planning/3d/ego_planner/planner/traj_utils/src/planning_visualization.cpp new file mode 100644 index 0000000..888320c --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/traj_utils/src/planning_visualization.cpp @@ -0,0 +1,265 @@ +#include + +using std::cout; +using std::endl; +namespace ego_planner +{ + PlanningVisualization::PlanningVisualization(ros::NodeHandle &nh) + { + node = nh; + + goal_point_pub = nh.advertise("goal_point", 2); + global_list_pub = nh.advertise("global_list", 2); + init_list_pub = nh.advertise("init_list", 2); + optimal_list_pub = nh.advertise("optimal_list", 2); + a_star_list_pub = nh.advertise("a_star_list", 20); + } + + // // real ids used: {id, id+1000} + void PlanningVisualization::displayMarkerList(ros::Publisher &pub, const vector &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 &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 &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 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> init_trajs, const double scale) + { + + if (init_list_pub.getNumSubscribers() == 0) + { + return; + } + + static int last_nums = 0; + + for ( int id=0; id blank; + displayMarkerList(init_list_pub, blank, scale, color, id, false); + ros::Duration(0.001).sleep(); + } + last_nums = 0; + + for ( int id=0; id 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 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> 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 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 &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 \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/planner/traj_utils/src/polynomial_traj.cpp b/motion_planning/3d/ego_planner/planner/traj_utils/src/polynomial_traj.cpp new file mode 100644 index 0000000..a83b7ae --- /dev/null +++ b/motion_planning/3d/ego_planner/planner/traj_utils/src/polynomial_traj.cpp @@ -0,0 +1,224 @@ +#include +#include + +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 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 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; +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/CMakeLists.txt new file mode 100644 index 0000000..a510ed1 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/CMakeLists.txt @@ -0,0 +1,13 @@ +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} +) + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/arch.cmake b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/arch.cmake new file mode 100644 index 0000000..9f1dcc8 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/arch.cmake @@ -0,0 +1,126 @@ +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() diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/cmake_modules.cmake b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/cmake_modules.cmake new file mode 100644 index 0000000..4370b38 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/cmake_modules.cmake @@ -0,0 +1,2 @@ +list(APPEND CMAKE_MODULE_PATH "${cmake_utils_SOURCE_PREFIX}/cmake_modules") +link_directories( ${cmake_utils_SOURCE_PREFIX}/lib/mosek8 ) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/color.cmake b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/color.cmake new file mode 100644 index 0000000..bada383 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake/color.cmake @@ -0,0 +1,17 @@ +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") diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindEigen.cmake b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindEigen.cmake new file mode 100644 index 0000000..d45fad7 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindEigen.cmake @@ -0,0 +1,173 @@ +# 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/] 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 _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) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindGSL.cmake b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindGSL.cmake new file mode 100644 index 0000000..797b245 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindGSL.cmake @@ -0,0 +1,135 @@ +# 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 ) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake new file mode 100644 index 0000000..d1a61b6 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake @@ -0,0 +1,124 @@ +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 _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() diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/package.xml b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/package.xml new file mode 100644 index 0000000..d745d2d --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/cmake_utils/package.xml @@ -0,0 +1,18 @@ + + + cmake_utils + 0.0.0 + + Once you used this package, + then you do not need to copy cmake files among packages + + + William.Wu + + LGPLv3 + + catkin + roscpp + cmake_modules + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server.zip b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server.zip new file mode 100644 index 0000000..33754b6 Binary files /dev/null and b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server.zip differ diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/CMakeLists.txt new file mode 100755 index 0000000..a89dd72 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/CMakeLists.txt @@ -0,0 +1,222 @@ +#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 +) + +############# +## 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) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/Makefile b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/Makefile new file mode 100755 index 0000000..b75b928 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map2D.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map2D.h new file mode 100755 index 0000000..cee4d65 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map2D.h @@ -0,0 +1,242 @@ +#ifndef MAP2D_H +#define MAP2D_H + +#include +#include +#include +#include + +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 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 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 _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 diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map3D.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map3D.h new file mode 100755 index 0000000..7f8aa7c --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/include/multi_map_server/Map3D.h @@ -0,0 +1,608 @@ +#ifndef MAP3D_H +#define MAP3D_H + +#include +#include +#include +#include +#include + +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::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& _grids) + { + _grids.clear(); + for (list::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::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::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::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::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::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 > lp; + for (list::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 > lp2; + for (list::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 >::iterator lend = lp.end(); + lend--; + for (list >::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 >::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::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 p1, pair p2) + { + if (p1.first != p2.first) + return (p1.first > p2.first); + else + return (p1.second > p2.second); + } + }; + + // List of vertical occupancy values + list 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& 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 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 _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 updateList; + + double originX, originY, originZ; + int mapX, mapY; + int expandStep; + vector mapBase; + + vector pts; + +}; +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/mainpage.dox b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/mainpage.dox new file mode 100755 index 0000000..3a1fb20 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b multi_map_server is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/MultiOccupancyGrid.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/MultiOccupancyGrid.msg new file mode 100755 index 0000000..d7b020d --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/MultiOccupancyGrid.msg @@ -0,0 +1,2 @@ +nav_msgs/OccupancyGrid[] maps +geometry_msgs/Pose[] origins diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/MultiSparseMap3D.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/MultiSparseMap3D.msg new file mode 100755 index 0000000..c7be2d7 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/MultiSparseMap3D.msg @@ -0,0 +1,2 @@ +SparseMap3D[] maps +geometry_msgs/Pose[] origins diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/SparseMap3D.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/SparseMap3D.msg new file mode 100755 index 0000000..777fb86 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/SparseMap3D.msg @@ -0,0 +1,4 @@ +Header header +nav_msgs/MapMetaData info +VerticalOccupancyGridList[] lists + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/VerticalOccupancyGridList.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/VerticalOccupancyGridList.msg new file mode 100755 index 0000000..379ffaa --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/msg/VerticalOccupancyGridList.msg @@ -0,0 +1,6 @@ +float32 x +float32 y +int32[] upper +int32[] lower +int32[] mass + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/package.xml b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/package.xml new file mode 100755 index 0000000..f037f19 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/package.xml @@ -0,0 +1,38 @@ + + multi_map_server + multi_map_server + 0.0.0 + Shaojie Shen + BSD + + http://ros.org/wiki/multi_map_server + + catkin + + roscpp + visualization_msgs + geometry_msgs + tf + nav_msgs + std_srvs + laser_geometry + pose_utils + message_generation + quadrotor_msgs + + roscpp + visualization_msgs + geometry_msgs + tf + nav_msgs + std_srvs + laser_geometry + pose_utils + message_runtime + quadrotor_msgs + + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_server/__init__.py b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_server/__init__.py new file mode 100755 index 0000000..b23e43d --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_server/__init__.py @@ -0,0 +1 @@ +#autogenerated by ROS python message generators \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiOccupancyGrid.py b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiOccupancyGrid.py new file mode 100755 index 0000000..bf2de49 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/_MultiOccupancyGrid.py @@ -0,0 +1,404 @@ +"""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(' 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(' 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(' 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") diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/__init__.py b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/__init__.py new file mode 100755 index 0000000..29e5795 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_server/msg/__init__.py @@ -0,0 +1,4 @@ +from ._SparseMap3D import * +from ._MultiOccupancyGrid import * +from ._MultiSparseMap3D import * +from ._VerticalOccupancyGridList import * diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_visualization.cc b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_visualization.cc new file mode 100755 index 0000000..7013fef --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/multi_map_visualization.cc @@ -0,0 +1,90 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +ros::Publisher pub1; +ros::Publisher pub2; + +vector maps2d; +vector origins2d; +vector maps3d; +vector 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 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("maps2d", 1, true); + pub2 = n.advertise("map3d", 1, true); + + ros::spin(); + return 0; +} + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/unused/multi_map_server.cc b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/unused/multi_map_server.cc new file mode 100755 index 0000000..a96d953 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/multi_map_server/src/unused/multi_map_server.cc @@ -0,0 +1,192 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 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(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(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(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("dmaps2d", 10, true); + pub2 = n.advertise( "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; +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/CMakeLists.txt new file mode 100755 index 0000000..554254a --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/CMakeLists.txt @@ -0,0 +1,213 @@ +#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 +) + +############# +## 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() diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/Makefile b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/Makefile new file mode 100755 index 0000000..b75b928 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/mainpage.dox b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/mainpage.dox new file mode 100755 index 0000000..3c09d69 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b odom_visualization is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh new file mode 100755 index 0000000..dcde17c Binary files /dev/null and b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh differ diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/package.xml b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/package.xml new file mode 100755 index 0000000..f875691 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/package.xml @@ -0,0 +1,31 @@ + + 0.0.0 + odom_visualization + + + odom_visualization + + + Shaojie Shen + BSD + http://ros.org/wiki/odom_visualization + + catkin + + roscpp + sensor_msgs + nav_msgs + visualization_msgs + tf + pose_utils + + roscpp + sensor_msgs + nav_msgs + visualization_msgs + tf + pose_utils + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp new file mode 100755 index 0000000..fcc9845 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp @@ -0,0 +1,483 @@ +#include +#include +#include "ros/ros.h" +#include "tf/transform_broadcaster.h" +#include "nav_msgs/Odometry.h" +#include "geometry_msgs/PoseWithCovarianceStamped.h" +#include "geometry_msgs/PoseStamped.h" +#include "nav_msgs/Path.h" +#include "sensor_msgs/Range.h" +#include "visualization_msgs/Marker.h" +#include "armadillo" +#include "pose_utils.h" +#include "quadrotor_msgs/PositionCommand.h" + +using namespace arma; +using namespace std; + +static string mesh_resource; +static double color_r, color_g, color_b, color_a, cov_scale, scale; + +bool cross_config = false; +bool tf45 = false; +bool cov_pos = false; +bool cov_vel = false; +bool cov_color = false; +bool origin = false; +bool isOriginSet = false; +colvec poseOrigin(6); +ros::Publisher posePub; +ros::Publisher pathPub; +ros::Publisher velPub; +ros::Publisher covPub; +ros::Publisher covVelPub; +ros::Publisher trajPub; +ros::Publisher sensorPub; +ros::Publisher meshPub; +ros::Publisher heightPub; +tf::TransformBroadcaster* broadcaster; +geometry_msgs::PoseStamped poseROS; +nav_msgs::Path pathROS; +visualization_msgs::Marker velROS; +visualization_msgs::Marker covROS; +visualization_msgs::Marker covVelROS; +visualization_msgs::Marker trajROS; +visualization_msgs::Marker sensorROS; +visualization_msgs::Marker meshROS; +sensor_msgs::Range heightROS; +string _frame_id; +int _drone_id; + +void odom_callback(const nav_msgs::Odometry::ConstPtr& msg) +{ + if (msg->header.frame_id == string("null")) + return; + + colvec pose(6); + pose(0) = msg->pose.pose.position.x; + pose(1) = msg->pose.pose.position.y; + pose(2) = msg->pose.pose.position.z; + colvec q(4); + + q(0) = msg->pose.pose.orientation.w; + q(1) = msg->pose.pose.orientation.x; + q(2) = msg->pose.pose.orientation.y; + q(3) = msg->pose.pose.orientation.z; + pose.rows(3,5) = R_to_ypr(quaternion_to_R(q)); + colvec vel(3); + + vel(0) = msg->twist.twist.linear.x; + vel(1) = msg->twist.twist.linear.y; + vel(2) = msg->twist.twist.linear.z; + + if (origin && !isOriginSet) + { + isOriginSet = true; + poseOrigin = pose; + } + if (origin) + { + vel = trans(ypr_to_R(pose.rows(3,5))) * vel; + pose = pose_update(pose_inverse(poseOrigin), pose); + vel = ypr_to_R(pose.rows(3,5)) * vel; + } + + // Pose + poseROS.header = msg->header; + poseROS.header.stamp = msg->header.stamp; + poseROS.header.frame_id = string("world"); + poseROS.pose.position.x = pose(0); + poseROS.pose.position.y = pose(1); + poseROS.pose.position.z = pose(2); + q = R_to_quaternion(ypr_to_R(pose.rows(3,5))); + poseROS.pose.orientation.w = q(0); + poseROS.pose.orientation.x = q(1); + poseROS.pose.orientation.y = q(2); + poseROS.pose.orientation.z = q(3); + posePub.publish(poseROS); + + // Velocity + colvec yprVel(3); + yprVel(0) = atan2(vel(1), vel(0)); + yprVel(1) = -atan2(vel(2), norm(vel.rows(0,1),2)); + yprVel(2) = 0; + q = R_to_quaternion(ypr_to_R(yprVel)); + velROS.header.frame_id = string("world"); + velROS.header.stamp = msg->header.stamp; + velROS.ns = string("velocity"); + velROS.id = 0; + velROS.type = visualization_msgs::Marker::ARROW; + velROS.action = visualization_msgs::Marker::ADD; + velROS.pose.position.x = pose(0); + velROS.pose.position.y = pose(1); + velROS.pose.position.z = pose(2); + velROS.pose.orientation.w = q(0); + velROS.pose.orientation.x = q(1); + velROS.pose.orientation.y = q(2); + velROS.pose.orientation.z = q(3); + velROS.scale.x = norm(vel, 2); + velROS.scale.y = 0.05; + velROS.scale.z = 0.05; + velROS.color.a = 1.0; + velROS.color.r = color_r; + velROS.color.g = color_g; + velROS.color.b = color_b; + velPub.publish(velROS); + + // Path + static ros::Time prevt = msg->header.stamp; + if ((msg->header.stamp - prevt).toSec() > 0.1) + { + prevt = msg->header.stamp; + pathROS.header = poseROS.header; + pathROS.poses.push_back(poseROS); + pathPub.publish(pathROS); + } + + // Covariance color + double r = 1; + double g = 1; + double b = 1; + bool G = msg->twist.covariance[33]; + bool V = msg->twist.covariance[34]; + bool L = msg->twist.covariance[35]; + if (cov_color) + { + r = G; + g = V; + b = L; + } + + // Covariance Position + if (cov_pos) + { + mat P(6,6); + for (int j = 0; j < 6; j++) + for (int i = 0; i < 6; i++) + P(i,j) = msg->pose.covariance[i+j*6]; + colvec eigVal; + mat eigVec; + eig_sym(eigVal, eigVec, P.submat(0,0,2,2)); + if (det(eigVec) < 0) + { + for (int k = 0; k < 3; k++) + { + mat eigVecRev = eigVec; + eigVecRev.col(k) *= -1; + if (det(eigVecRev) > 0) + { + eigVec = eigVecRev; + break; + } + } + } + covROS.header.frame_id = string("world"); + covROS.header.stamp = msg->header.stamp; + covROS.ns = string("covariance"); + covROS.id = 0; + covROS.type = visualization_msgs::Marker::SPHERE; + covROS.action = visualization_msgs::Marker::ADD; + covROS.pose.position.x = pose(0); + covROS.pose.position.y = pose(1); + covROS.pose.position.z = pose(2); + q = R_to_quaternion(eigVec); + covROS.pose.orientation.w = q(0); + covROS.pose.orientation.x = q(1); + covROS.pose.orientation.y = q(2); + covROS.pose.orientation.z = q(3); + covROS.scale.x = sqrt(eigVal(0))*cov_scale; + covROS.scale.y = sqrt(eigVal(1))*cov_scale; + covROS.scale.z = sqrt(eigVal(2))*cov_scale; + covROS.color.a = 0.4; + covROS.color.r = r * 0.5; + covROS.color.g = g * 0.5; + covROS.color.b = b * 0.5; + covPub.publish(covROS); + } + + // Covariance Velocity + if (cov_vel) + { + mat P(3,3); + for (int j = 0; j < 3; j++) + for (int i = 0; i < 3; i++) + P(i,j) = msg->twist.covariance[i+j*6]; + mat R = ypr_to_R(pose.rows(3,5)); + P = R * P * trans(R); + colvec eigVal; + mat eigVec; + eig_sym(eigVal, eigVec, P); + if (det(eigVec) < 0) + { + for (int k = 0; k < 3; k++) + { + mat eigVecRev = eigVec; + eigVecRev.col(k) *= -1; + if (det(eigVecRev) > 0) + { + eigVec = eigVecRev; + break; + } + } + } + covVelROS.header.frame_id = string("world"); + covVelROS.header.stamp = msg->header.stamp; + covVelROS.ns = string("covariance_velocity"); + covVelROS.id = 0; + covVelROS.type = visualization_msgs::Marker::SPHERE; + covVelROS.action = visualization_msgs::Marker::ADD; + covVelROS.pose.position.x = pose(0); + covVelROS.pose.position.y = pose(1); + covVelROS.pose.position.z = pose(2); + q = R_to_quaternion(eigVec); + covVelROS.pose.orientation.w = q(0); + covVelROS.pose.orientation.x = q(1); + covVelROS.pose.orientation.y = q(2); + covVelROS.pose.orientation.z = q(3); + covVelROS.scale.x = sqrt(eigVal(0))*cov_scale; + covVelROS.scale.y = sqrt(eigVal(1))*cov_scale; + covVelROS.scale.z = sqrt(eigVal(2))*cov_scale; + covVelROS.color.a = 0.4; + covVelROS.color.r = r; + covVelROS.color.g = g; + covVelROS.color.b = b; + covVelPub.publish(covVelROS); + } + + // Color Coded Trajectory + static colvec ppose = pose; + static ros::Time pt = msg->header.stamp; + ros::Time t = msg->header.stamp; + if ((t - pt).toSec() > 0.5) + { + trajROS.header.frame_id = string("world"); + trajROS.header.stamp = ros::Time::now(); + trajROS.ns = string("trajectory"); + trajROS.type = visualization_msgs::Marker::LINE_LIST; + trajROS.action = visualization_msgs::Marker::ADD; + trajROS.pose.position.x = 0; + trajROS.pose.position.y = 0; + trajROS.pose.position.z = 0; + trajROS.pose.orientation.w = 1; + trajROS.pose.orientation.x = 0; + trajROS.pose.orientation.y = 0; + trajROS.pose.orientation.z = 0; + trajROS.scale.x = 0.1; + trajROS.scale.y = 0; + trajROS.scale.z = 0; + trajROS.color.r = 0.0; + trajROS.color.g = 1.0; + trajROS.color.b = 0.0; + trajROS.color.a = 0.8; + geometry_msgs::Point p; + p.x = ppose(0); + p.y = ppose(1); + p.z = ppose(2); + trajROS.points.push_back(p); + p.x = pose(0); + p.y = pose(1); + p.z = pose(2); + trajROS.points.push_back(p); + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = 1; + trajROS.colors.push_back(color); + trajROS.colors.push_back(color); + ppose = pose; + pt = t; + trajPub.publish(trajROS); + } + + // Sensor availability + sensorROS.header.frame_id = string("world"); + sensorROS.header.stamp = msg->header.stamp; + sensorROS.ns = string("sensor"); + sensorROS.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + sensorROS.action = visualization_msgs::Marker::ADD; + sensorROS.pose.position.x = pose(0); + sensorROS.pose.position.y = pose(1); + sensorROS.pose.position.z = pose(2) + 1.0; + sensorROS.pose.orientation.w = q(0); + sensorROS.pose.orientation.x = q(1); + sensorROS.pose.orientation.y = q(2); + sensorROS.pose.orientation.z = q(3); + string strG = G?string(" GPS "):string(""); + string strV = V?string(" Vision "):string(""); + string strL = L?string(" Laser "):string(""); + sensorROS.text = "| " + strG + strV + strL + " |"; + sensorROS.color.a = 1.0; + sensorROS.color.r = 1.0; + sensorROS.color.g = 1.0; + sensorROS.color.b = 1.0; + sensorROS.scale.z = 0.5; + sensorPub.publish(sensorROS); + + // Laser height measurement + double H = msg->twist.covariance[32]; + heightROS.header.frame_id = string("height"); + heightROS.header.stamp = msg->header.stamp; + heightROS.radiation_type = sensor_msgs::Range::ULTRASOUND; + heightROS.field_of_view = 5.0 * M_PI / 180.0; + heightROS.min_range = -100; + heightROS.max_range = 100; + heightROS.range = H; + heightPub.publish(heightROS); + + // Mesh model + meshROS.header.frame_id = _frame_id; + meshROS.header.stamp = msg->header.stamp; + meshROS.ns = "mesh"; + meshROS.id = 0; + meshROS.type = visualization_msgs::Marker::MESH_RESOURCE; + meshROS.action = visualization_msgs::Marker::ADD; + meshROS.pose.position.x = msg->pose.pose.position.x; + meshROS.pose.position.y = msg->pose.pose.position.y; + meshROS.pose.position.z = msg->pose.pose.position.z; + q(0) = msg->pose.pose.orientation.w; + q(1) = msg->pose.pose.orientation.x; + q(2) = msg->pose.pose.orientation.y; + q(3) = msg->pose.pose.orientation.z; + if (cross_config) + { + colvec ypr = R_to_ypr(quaternion_to_R(q)); + ypr(0) += 45.0*PI/180.0; + q = R_to_quaternion(ypr_to_R(ypr)); + } + meshROS.pose.orientation.w = q(0); + meshROS.pose.orientation.x = q(1); + meshROS.pose.orientation.y = q(2); + meshROS.pose.orientation.z = q(3); + meshROS.scale.x = scale; + meshROS.scale.y = scale; + meshROS.scale.z = scale; + meshROS.color.a = color_a; + meshROS.color.r = color_r; + meshROS.color.g = color_g; + meshROS.color.b = color_b; + meshROS.mesh_resource = mesh_resource; + meshPub.publish(meshROS); + + // TF for raw sensor visualization + if (tf45) + { + tf::Transform transform; + transform.setOrigin(tf::Vector3(pose(0), pose(1), pose(2))); + transform.setRotation(tf::Quaternion(q(1), q(2), q(3), q(0))); + + tf::Transform transform45; + transform45.setOrigin(tf::Vector3(0, 0, 0)); + colvec y45 = zeros(3); + y45(0) = 45.0 * M_PI/180; + colvec q45 = R_to_quaternion(ypr_to_R(y45)); + transform45.setRotation(tf::Quaternion(q45(1), q45(2), q45(3), q45(0))); + + tf::Transform transform90; + transform90.setOrigin(tf::Vector3(0, 0, 0)); + colvec p90 = zeros(3); + p90(1) = 90.0 * M_PI/180; + colvec q90 = R_to_quaternion(ypr_to_R(p90)); + transform90.setRotation(tf::Quaternion(q90(1), q90(2), q90(3), q90(0))); + + string base_s = _drone_id == -1 ? string("base") : string("base") +std::to_string(_drone_id); + string laser_s = _drone_id == -1 ? string("laser") : string("laser") +std::to_string(_drone_id); + string vision_s = _drone_id == -1 ? string("vision") : string("vision")+std::to_string(_drone_id); + string height_s = _drone_id == -1 ? string("height") : string("height")+std::to_string(_drone_id); + + + broadcaster->sendTransform(tf::StampedTransform(transform, msg->header.stamp, string("world"), base_s)); + broadcaster->sendTransform(tf::StampedTransform(transform45, msg->header.stamp, base_s, laser_s)); + broadcaster->sendTransform(tf::StampedTransform(transform45, msg->header.stamp, base_s, vision_s)); + broadcaster->sendTransform(tf::StampedTransform(transform90, msg->header.stamp, base_s, height_s)); + } +} + +void cmd_callback(const quadrotor_msgs::PositionCommand cmd) +{ + if (cmd.header.frame_id == string("null")) + return; + + colvec pose(6); + pose(0) = cmd.position.x; + pose(1) = cmd.position.y; + pose(2) = cmd.position.z; + colvec q(4); + q(0) = 1.0; + q(1) = 0.0; + q(2) = 0.0; + q(3) = 0.0; + pose.rows(3,5) = R_to_ypr(quaternion_to_R(q)); + + // Mesh model + meshROS.header.frame_id = _frame_id; + meshROS.header.stamp = cmd.header.stamp; + meshROS.ns = "mesh"; + meshROS.id = 0; + meshROS.type = visualization_msgs::Marker::MESH_RESOURCE; + meshROS.action = visualization_msgs::Marker::ADD; + meshROS.pose.position.x = cmd.position.x; + meshROS.pose.position.y = cmd.position.y; + meshROS.pose.position.z = cmd.position.z; + + if (cross_config) + { + colvec ypr = R_to_ypr(quaternion_to_R(q)); + ypr(0) += 45.0*PI/180.0; + q = R_to_quaternion(ypr_to_R(ypr)); + } + meshROS.pose.orientation.w = q(0); + meshROS.pose.orientation.x = q(1); + meshROS.pose.orientation.y = q(2); + meshROS.pose.orientation.z = q(3); + meshROS.scale.x = 2.0; + meshROS.scale.y = 2.0; + meshROS.scale.z = 2.0; + meshROS.color.a = color_a; + meshROS.color.r = color_r; + meshROS.color.g = color_g; + meshROS.color.b = color_b; + meshROS.mesh_resource = mesh_resource; + meshPub.publish(meshROS); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "odom_visualization"); + ros::NodeHandle n("~"); + + n.param("mesh_resource", mesh_resource, std::string("package://odom_visualization/meshes/hummingbird.mesh")); + n.param("color/r", color_r, 1.0); + n.param("color/g", color_g, 0.0); + n.param("color/b", color_b, 0.0); + n.param("color/a", color_a, 1.0); + n.param("origin", origin, false); + n.param("robot_scale", scale, 2.0); + n.param("frame_id", _frame_id, string("world") ); + + n.param("cross_config", cross_config, false); + n.param("tf45", tf45, false); + n.param("covariance_scale", cov_scale, 100.0); + n.param("covariance_position", cov_pos, false); + n.param("covariance_velocity", cov_vel, false); + n.param("covariance_color", cov_color, false); + n.param("drone_id", _drone_id, -1); + + ros::Subscriber sub_odom = n.subscribe("odom", 100, odom_callback); + ros::Subscriber sub_cmd = n.subscribe("cmd", 100, cmd_callback); + posePub = n.advertise("pose", 100, true); + pathPub = n.advertise( "path", 100, true); + velPub = n.advertise("velocity", 100, true); + covPub = n.advertise("covariance", 100, true); + covVelPub = n.advertise("covariance_velocity", 100, true); + trajPub = n.advertise("trajectory", 100, true); + sensorPub = n.advertise("sensor", 100, true); + meshPub = n.advertise("robot", 100, true); + heightPub = n.advertise( "height", 100, true); + tf::TransformBroadcaster b; + broadcaster = &b; + + ros::spin(); + + return 0; +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp~ b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp~ new file mode 100755 index 0000000..39f7454 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/odom_visualization/src/odom_visualization.cpp~ @@ -0,0 +1,417 @@ +#include +#include +#include "ros/ros.h" +#include "tf/transform_broadcaster.h" +#include "nav_msgs/Odometry.h" +#include "geometry_msgs/PoseWithCovarianceStamped.h" +#include "geometry_msgs/PoseStamped.h" +#include "nav_msgs/Path.h" +#include "sensor_msgs/Range.h" +#include "visualization_msgs/Marker.h" +#include "armadillo" +#include "pose_utils.h" + +using namespace arma; +using namespace std; + +static string mesh_resource; +static double color_r, color_g, color_b, color_a, cov_scale; +bool cross_config = false; +bool tf45 = false; +bool cov_pos = false; +bool cov_vel = false; +bool cov_color = false; +bool origin = false; +bool isOriginSet = false; +colvec poseOrigin(6); +ros::Publisher posePub; +ros::Publisher pathPub; +ros::Publisher velPub; +ros::Publisher covPub; +ros::Publisher covVelPub; +ros::Publisher trajPub; +ros::Publisher sensorPub; +ros::Publisher meshPub; +ros::Publisher heightPub; +tf::TransformBroadcaster* broadcaster; +geometry_msgs::PoseStamped poseROS; +nav_msgs::Path pathROS; +visualization_msgs::Marker velROS; +visualization_msgs::Marker covROS; +visualization_msgs::Marker covVelROS; +visualization_msgs::Marker trajROS; +visualization_msgs::Marker sensorROS; +visualization_msgs::Marker meshROS; +sensor_msgs::Range heightROS; + +void odom_callback(const nav_msgs::Odometry::ConstPtr& msg) +{ + if (msg->header.frame_id == string("null")) + return; + colvec pose(6); + pose(0) = msg->pose.pose.position.x; + pose(1) = msg->pose.pose.position.y; + pose(2) = msg->pose.pose.position.z; + colvec q(4); + q(0) = msg->pose.pose.orientation.w; + q(1) = msg->pose.pose.orientation.x; + q(2) = msg->pose.pose.orientation.y; + q(3) = msg->pose.pose.orientation.z; + pose.rows(3,5) = R_to_ypr(quaternion_to_R(q)); + colvec vel(3); + vel(0) = msg->twist.twist.linear.x; + vel(1) = msg->twist.twist.linear.y; + vel(2) = msg->twist.twist.linear.z; + + if (origin && !isOriginSet) + { + isOriginSet = true; + poseOrigin = pose; + } + if (origin) + { + vel = trans(ypr_to_R(pose.rows(3,5))) * vel; + pose = pose_update(pose_inverse(poseOrigin), pose); + vel = ypr_to_R(pose.rows(3,5)) * vel; + } + + // Pose + poseROS.header = msg->header; + poseROS.header.stamp = msg->header.stamp; + poseROS.header.frame_id = string("/map"); + poseROS.pose.position.x = pose(0); + poseROS.pose.position.y = pose(1); + poseROS.pose.position.z = pose(2); + q = R_to_quaternion(ypr_to_R(pose.rows(3,5))); + poseROS.pose.orientation.w = q(0); + poseROS.pose.orientation.x = q(1); + poseROS.pose.orientation.y = q(2); + poseROS.pose.orientation.z = q(3); + posePub.publish(poseROS); + + // Velocity + colvec yprVel(3); + yprVel(0) = atan2(vel(1), vel(0)); + yprVel(1) = -atan2(vel(2), norm(vel.rows(0,1),2)); + yprVel(2) = 0; + q = R_to_quaternion(ypr_to_R(yprVel)); + velROS.header.frame_id = string("/map"); + velROS.header.stamp = msg->header.stamp; + velROS.ns = string("velocity"); + velROS.id = 0; + velROS.type = visualization_msgs::Marker::ARROW; + velROS.action = visualization_msgs::Marker::ADD; + velROS.pose.position.x = pose(0); + velROS.pose.position.y = pose(1); + velROS.pose.position.z = pose(2); + velROS.pose.orientation.w = q(0); + velROS.pose.orientation.x = q(1); + velROS.pose.orientation.y = q(2); + velROS.pose.orientation.z = q(3); + velROS.scale.x = norm(vel, 2); + velROS.scale.y = 0.05; + velROS.scale.z = 0.05; + velROS.color.a = 1.0; + velROS.color.r = color_r; + velROS.color.g = color_g; + velROS.color.b = color_b; + velPub.publish(velROS); + + // Path + static ros::Time prevt = msg->header.stamp; + if ((msg->header.stamp - prevt).toSec() > 0.1) + { + prevt = msg->header.stamp; + pathROS.header = poseROS.header; + pathROS.poses.push_back(poseROS); + pathPub.publish(pathROS); + } + + // Covariance color + double r = 1; + double g = 1; + double b = 1; + bool G = msg->twist.covariance[33]; + bool V = msg->twist.covariance[34]; + bool L = msg->twist.covariance[35]; + if (cov_color) + { + r = G; + g = V; + b = L; + } + + // Covariance Position + if (cov_pos) + { + mat P(6,6); + for (int j = 0; j < 6; j++) + for (int i = 0; i < 6; i++) + P(i,j) = msg->pose.covariance[i+j*6]; + colvec eigVal; + mat eigVec; + eig_sym(eigVal, eigVec, P.submat(0,0,2,2)); + if (det(eigVec) < 0) + { + for (int k = 0; k < 3; k++) + { + mat eigVecRev = eigVec; + eigVecRev.col(k) *= -1; + if (det(eigVecRev) > 0) + { + eigVec = eigVecRev; + break; + } + } + } + covROS.header.frame_id = string("/map"); + covROS.header.stamp = msg->header.stamp; + covROS.ns = string("covariance"); + covROS.id = 0; + covROS.type = visualization_msgs::Marker::SPHERE; + covROS.action = visualization_msgs::Marker::ADD; + covROS.pose.position.x = pose(0); + covROS.pose.position.y = pose(1); + covROS.pose.position.z = pose(2); + q = R_to_quaternion(eigVec); + covROS.pose.orientation.w = q(0); + covROS.pose.orientation.x = q(1); + covROS.pose.orientation.y = q(2); + covROS.pose.orientation.z = q(3); + covROS.scale.x = sqrt(eigVal(0))*cov_scale; + covROS.scale.y = sqrt(eigVal(1))*cov_scale; + covROS.scale.z = sqrt(eigVal(2))*cov_scale; + covROS.color.a = 0.4; + covROS.color.r = r * 0.5; + covROS.color.g = g * 0.5; + covROS.color.b = b * 0.5; + covPub.publish(covROS); + } + + // Covariance Velocity + if (cov_vel) + { + mat P(3,3); + for (int j = 0; j < 3; j++) + for (int i = 0; i < 3; i++) + P(i,j) = msg->twist.covariance[i+j*6]; + mat R = ypr_to_R(pose.rows(3,5)); + P = R * P * trans(R); + colvec eigVal; + mat eigVec; + eig_sym(eigVal, eigVec, P); + if (det(eigVec) < 0) + { + for (int k = 0; k < 3; k++) + { + mat eigVecRev = eigVec; + eigVecRev.col(k) *= -1; + if (det(eigVecRev) > 0) + { + eigVec = eigVecRev; + break; + } + } + } + covVelROS.header.frame_id = string("/map"); + covVelROS.header.stamp = msg->header.stamp; + covVelROS.ns = string("covariance_velocity"); + covVelROS.id = 0; + covVelROS.type = visualization_msgs::Marker::SPHERE; + covVelROS.action = visualization_msgs::Marker::ADD; + covVelROS.pose.position.x = pose(0); + covVelROS.pose.position.y = pose(1); + covVelROS.pose.position.z = pose(2); + q = R_to_quaternion(eigVec); + covVelROS.pose.orientation.w = q(0); + covVelROS.pose.orientation.x = q(1); + covVelROS.pose.orientation.y = q(2); + covVelROS.pose.orientation.z = q(3); + covVelROS.scale.x = sqrt(eigVal(0))*cov_scale; + covVelROS.scale.y = sqrt(eigVal(1))*cov_scale; + covVelROS.scale.z = sqrt(eigVal(2))*cov_scale; + covVelROS.color.a = 0.4; + covVelROS.color.r = r; + covVelROS.color.g = g; + covVelROS.color.b = b; + covVelPub.publish(covVelROS); + } + + // Color Coded Trajectory + static colvec ppose = pose; + static ros::Time pt = msg->header.stamp; + ros::Time t = msg->header.stamp; + if ((t - pt).toSec() > 0.5) + { + trajROS.header.frame_id = string("/map"); + trajROS.header.stamp = ros::Time::now(); + trajROS.ns = string("trajectory"); + trajROS.type = visualization_msgs::Marker::LINE_LIST; + trajROS.action = visualization_msgs::Marker::ADD; + trajROS.pose.position.x = 0; + trajROS.pose.position.y = 0; + trajROS.pose.position.z = 0; + trajROS.pose.orientation.w = 1; + trajROS.pose.orientation.x = 0; + trajROS.pose.orientation.y = 0; + trajROS.pose.orientation.z = 0; + trajROS.scale.x = 0.1; + trajROS.scale.y = 0; + trajROS.scale.z = 0; + trajROS.color.r = 0.0; + trajROS.color.g = 1.0; + trajROS.color.b = 0.0; + trajROS.color.a = 0.8; + geometry_msgs::Point p; + p.x = ppose(0); + p.y = ppose(1); + p.z = ppose(2); + trajROS.points.push_back(p); + p.x = pose(0); + p.y = pose(1); + p.z = pose(2); + trajROS.points.push_back(p); + std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = 1; + trajROS.colors.push_back(color); + trajROS.colors.push_back(color); + ppose = pose; + pt = t; + trajPub.publish(trajROS); + } + + // Sensor availability + sensorROS.header.frame_id = string("/map"); + sensorROS.header.stamp = msg->header.stamp; + sensorROS.ns = string("sensor"); + sensorROS.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + sensorROS.action = visualization_msgs::Marker::ADD; + sensorROS.pose.position.x = pose(0); + sensorROS.pose.position.y = pose(1); + sensorROS.pose.position.z = pose(2) + 1.0; + sensorROS.pose.orientation.w = q(0); + sensorROS.pose.orientation.x = q(1); + sensorROS.pose.orientation.y = q(2); + sensorROS.pose.orientation.z = q(3); + string strG = G?string(" GPS "):string(""); + string strV = V?string(" Vision "):string(""); + string strL = L?string(" Laser "):string(""); + sensorROS.text = "| " + strG + strV + strL + " |"; + sensorROS.color.a = 1.0; + sensorROS.color.r = 1.0; + sensorROS.color.g = 1.0; + sensorROS.color.b = 1.0; + sensorROS.scale.z = 0.5; + sensorPub.publish(sensorROS); + + // Laser height measurement + double H = msg->twist.covariance[32]; + heightROS.header.frame_id = string("height"); + heightROS.header.stamp = msg->header.stamp; + heightROS.radiation_type = sensor_msgs::Range::ULTRASOUND; + heightROS.field_of_view = 5.0 * M_PI / 180.0; + heightROS.min_range = -100; + heightROS.max_range = 100; + heightROS.range = H; + heightPub.publish(heightROS); + + // Mesh model + meshROS.header.frame_id = string("/map"); + meshROS.header.stamp = msg->header.stamp; + meshROS.ns = "mesh"; + meshROS.id = 0; + meshROS.type = visualization_msgs::Marker::MESH_RESOURCE; + meshROS.action = visualization_msgs::Marker::ADD; + meshROS.pose.position.x = msg->pose.pose.position.x; + meshROS.pose.position.y = msg->pose.pose.position.y; + meshROS.pose.position.z = msg->pose.pose.position.z; + q(0) = msg->pose.pose.orientation.w; + q(1) = msg->pose.pose.orientation.x; + q(2) = msg->pose.pose.orientation.y; + q(3) = msg->pose.pose.orientation.z; + if (cross_config) + { + colvec ypr = R_to_ypr(quaternion_to_R(q)); + ypr(0) += 45.0*PI/180.0; + q = R_to_quaternion(ypr_to_R(ypr)); + } + meshROS.pose.orientation.w = q(0); + meshROS.pose.orientation.x = q(1); + meshROS.pose.orientation.y = q(2); + meshROS.pose.orientation.z = q(3); + meshROS.scale.x = 1.5; + meshROS.scale.y = 1.5; + meshROS.scale.z = 1.5; + meshROS.color.a = color_a; + meshROS.color.r = color_r; + meshROS.color.g = color_g; + meshROS.color.b = color_b; + meshROS.mesh_resource = mesh_resource; + meshPub.publish(meshROS); + + // TF for raw sensor visualization + if (tf45) + { + tf::Transform transform; + transform.setOrigin(tf::Vector3(pose(0), pose(1), pose(2))); + transform.setRotation(tf::Quaternion(q(1), q(2), q(3), q(0))); + + tf::Transform transform45; + transform45.setOrigin(tf::Vector3(0, 0, 0)); + colvec y45 = zeros(3); + y45(0) = 45.0 * M_PI/180; + colvec q45 = R_to_quaternion(ypr_to_R(y45)); + transform45.setRotation(tf::Quaternion(q45(1), q45(2), q45(3), q45(0))); + + tf::Transform transform90; + transform90.setOrigin(tf::Vector3(0, 0, 0)); + colvec p90 = zeros(3); + p90(1) = 90.0 * M_PI/180; + colvec q90 = R_to_quaternion(ypr_to_R(p90)); + transform90.setRotation(tf::Quaternion(q90(1), q90(2), q90(3), q90(0))); + + broadcaster->sendTransform(tf::StampedTransform(transform, msg->header.stamp, string("/map"), string("/base"))); + broadcaster->sendTransform(tf::StampedTransform(transform45, msg->header.stamp, string("/base"), string("/laser"))); + broadcaster->sendTransform(tf::StampedTransform(transform45, msg->header.stamp, string("/base"), string("/vision"))); + broadcaster->sendTransform(tf::StampedTransform(transform90, msg->header.stamp, string("/base"), string("/height"))); + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "odom_visualization"); + ros::NodeHandle n("~"); + + n.param("mesh_resource", mesh_resource, std::string("package://odom_visualization/meshes/hummingbird.mesh")); + n.param("color/r", color_r, 1.0); + n.param("color/g", color_g, 0.0); + n.param("color/b", color_b, 0.0); + n.param("color/a", color_a, 1.0); + n.param("origin", origin, false); + n.param("cross_config", cross_config, false); + n.param("tf45", tf45, false); + n.param("covariance_scale", cov_scale, 100.0); + n.param("covariance_position", cov_pos, false); + n.param("covariance_velocity", cov_vel, false); + n.param("covariance_color", cov_color, false); + + ros::Subscriber sub = n.subscribe("odom", 100, odom_callback); + posePub = n.advertise("pose", 100, true); + pathPub = n.advertise( "path", 100, true); + velPub = n.advertise("velocity", 100, true); + covPub = n.advertise("covariance", 100, true); + covVelPub = n.advertise("covariance_velocity", 100, true); + trajPub = n.advertise("trajectory", 100, true); + sensorPub = n.advertise("sensor", 100, true); + meshPub = n.advertise("robot", 100, true); + heightPub = n.advertise( "height", 100, true); + tf::TransformBroadcaster b; + broadcaster = &b; + + ros::spin(); + + return 0; +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/CMakeLists.txt new file mode 100755 index 0000000..d65f182 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 2.8.3) +project(pose_utils) + +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_package(catkin REQUIRED COMPONENTS + #armadillo + roscpp +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES pose_utils +# CATKIN_DEPENDS geometry_msgs nav_msgs +# DEPENDS system_lib +) + +find_package(Armadillo REQUIRED) + +include_directories( + ${catkin_INCLUDE_DIRS} + ${ARMADILLO_INCLUDE_DIRS} + include + ) + +add_library(pose_utils + ${ARMADILLO_LIBRARIES} + src/pose_utils.cpp) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/Makefile b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/Makefile new file mode 100755 index 0000000..b75b928 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/include/pose_utils.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/include/pose_utils.h new file mode 100755 index 0000000..326c7c8 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/include/pose_utils.h @@ -0,0 +1,53 @@ +#ifndef POSE_UTILS_H +#define POSE_UTILS_H + +#include +#include "armadillo" + +#define PI 3.14159265359 +#define NUM_INF 999999.9 + +using namespace arma; +using namespace std; + +// Rotation --------------------- +mat ypr_to_R(const colvec& ypr); + +mat yaw_to_R(double yaw); + +colvec R_to_ypr(const mat& R); + +mat quaternion_to_R(const colvec& q); + +colvec R_to_quaternion(const mat& R); + +colvec quaternion_mul(const colvec& q1, const colvec& q2); + +colvec quaternion_inv(const colvec& q); + +// General Pose Update ---------- +colvec pose_update(const colvec& X1, const colvec& X2); + +colvec pose_inverse(const colvec& X); + +colvec pose_update_2d(const colvec& X1, const colvec& X2); + +colvec pose_inverse_2d(const colvec& X); + +// For Pose EKF ----------------- +mat Jplus1(const colvec& X1, const colvec& X2); + +mat Jplus2(const colvec& X1, const colvec& X2); + +// For IMU EKF ------------------ +colvec state_update(const colvec& X, const colvec& U, double dt); + +mat jacobianF(const colvec& X, const colvec& U, double dt); + +mat jacobianU(const colvec& X, const colvec& U, double dt); + +colvec state_measure(const colvec& X); + +mat jacobianH(); + +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/package.xml b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/package.xml new file mode 100755 index 0000000..df593f9 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/package.xml @@ -0,0 +1,15 @@ + + pose_utils + 0.0.0 + pose_utils + Shaojie Shen + BSD + catkin + roscpp + roscpp + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/src/pose_utils.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/src/pose_utils.cpp new file mode 100755 index 0000000..f2b4365 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/src/pose_utils.cpp @@ -0,0 +1,531 @@ +#include "pose_utils.h" + +// Rotation --------------------- + +mat ypr_to_R(const colvec& ypr) +{ + double c, s; + mat Rz = zeros(3,3); + double y = ypr(0); + c = cos(y); + s = sin(y); + Rz(0,0) = c; + Rz(1,0) = s; + Rz(0,1) = -s; + Rz(1,1) = c; + Rz(2,2) = 1; + + mat Ry = zeros(3,3); + double p = ypr(1); + c = cos(p); + s = sin(p); + Ry(0,0) = c; + Ry(2,0) = -s; + Ry(0,2) = s; + Ry(2,2) = c; + Ry(1,1) = 1; + + mat Rx = zeros(3,3); + double r = ypr(2); + c = cos(r); + s = sin(r); + Rx(1,1) = c; + Rx(2,1) = s; + Rx(1,2) = -s; + Rx(2,2) = c; + Rx(0,0) = 1; + + mat R = Rz*Ry*Rx; + return R; +} + +mat yaw_to_R(double yaw) +{ + mat R = zeros(2,2); + double c = cos(yaw); + double s = sin(yaw); + R(0,0) = c; + R(1,0) = s; + R(0,1) = -s; + R(1,1) = c; + return R; +} + +colvec R_to_ypr(const mat& R) +{ + colvec n = R.col(0); + colvec o = R.col(1); + colvec a = R.col(2); + + colvec ypr(3); + double y = atan2(n(1), n(0)); + double p = atan2(-n(2), n(0)*cos(y)+n(1)*sin(y)); + double r = atan2(a(0)*sin(y)-a(1)*cos(y), -o(0)*sin(y)+o(1)*cos(y)); + ypr(0) = y; + ypr(1) = p; + ypr(2) = r; + + return ypr; +} + +mat quaternion_to_R(const colvec& q) +{ + double n = norm(q, 2); + colvec nq = q / n; + + double w = nq(0); + double x = nq(1); + double y = nq(2); + double z = nq(3); + double w2 = w*w; + double x2 = x*x; + double y2 = y*y; + double z2 = z*z; + double xy = x*y; + double xz = x*z; + double yz = y*z; + double wx = w*x; + double wy = w*y; + double wz = w*z; + + mat R = zeros(3,3); + R(0,0) = w2+x2-y2-z2; + R(1,0) = 2*(wz + xy); + R(2,0) = 2*(xz - wy); + R(0,1) = 2*(xy - wz); + R(1,1) = w2-x2+y2-z2; + R(2,1) = 2*(wx + yz); + R(0,2) = 2*(wy + xz); + R(1,2) = 2*(yz - wx); + R(2,2) = w2-x2-y2+z2; + return R; +} + +colvec R_to_quaternion(const mat& R) +{ + colvec q(4); + double tr = R(0,0) + R(1,1) + R(2,2); + if (tr > 0) + { + double S = sqrt(tr + 1.0) * 2; + q(0) = 0.25 * S; + q(1) = (R(2,1) - R(1,2)) / S; + q(2) = (R(0,2) - R(2,0)) / S; + q(3) = (R(1,0) - R(0,1)) / S; + } + else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) + { + double S = sqrt(1.0 + R(0,0) - R(1,1) - R(2,2)) * 2; + q(0) = (R(2,1) - R(1,2)) / S; + q(1) = 0.25 * S; + q(2) = (R(0,1) + R(1,0)) / S; + q(3) = (R(0,2) + R(2,0)) / S; + } + else if (R(1,1) > R(2,2)) + { + double S = sqrt(1.0 + R(1,1) - R(0,0) - R(2,2)) * 2; + q(0) = (R(0,2) - R(2,0)) / S; + q(1) = (R(0,1) + R(1,0)) / S; + q(2) = 0.25 * S; + q(3) = (R(1,2) + R(2,1)) / S; + } + else + { + double S = sqrt(1.0 + R(2,2) - R(0,0) - R(1,1)) * 2; + q(0) = (R(1,0) - R(0,1)) / S; + q(1) = (R(0,2) + R(2,0)) / S; + q(2) = (R(1,2) + R(2,1)) / S; + q(3) = 0.25 * S; + } + return q; +} + +colvec quaternion_mul(const colvec& q1, const colvec& q2) +{ + double a1 = q1(0); + double b1 = q1(1); + double c1 = q1(2); + double d1 = q1(3); + + double a2 = q2(0); + double b2 = q2(1); + double c2 = q2(2); + double d2 = q2(3); + + colvec q3(4); + q3(0) = a1*a2 - b1*b2 - c1*c2 - d1*d2; + q3(1) = a1*b2 + b1*a2 + c1*d2 - d1*c2; + q3(2) = a1*c2 - b1*d2 + c1*a2 + d1*b2; + q3(3) = a1*d2 + b1*c2 - c1*b2 + d1*a2; + return q3; +} + +colvec quaternion_inv(const colvec& q) +{ + colvec q2(4); + q2(0) = q(0); + q2(1) = -q(1); + q2(2) = -q(2); + q2(3) = -q(3); + return q2; +} + +// General Pose Update ---------- + +colvec pose_update(const colvec& X1, const colvec& X2) +{ + mat R1 = ypr_to_R(X1.rows(3,5)); + mat R2 = ypr_to_R(X2.rows(3,5)); + mat R3 = R1 * R2; + + colvec X3xyz = X1.rows(0,2) + R1*X2.rows(0,2); + colvec X3ypr = R_to_ypr(R3); + + colvec X3 = join_cols(X3xyz, X3ypr); + return X3; +} + +colvec pose_inverse(const colvec& X) +{ + mat R = ypr_to_R(X.rows(3,5)); + colvec n = R.col(0); + colvec o = R.col(1); + colvec a = R.col(2); + + colvec XIxyz = -trans(R) * (X.rows(0,2)); + colvec XIypr(3); + double XIy = atan2(o(0), n(0)); + double XIp = atan2(-a(0), n(0)*cos(XIy)+o(0)*sin(XIy)); + double XIr = atan2(n(2)*sin(XIy)-o(2)*cos(XIy), -n(1)*sin(XIy)+o(1)*cos(XIy)); + XIypr(0) = XIy; + XIypr(1) = XIp; + XIypr(2) = XIr; + + colvec XI = join_cols(XIxyz, XIypr); + return XI; +} + +colvec pose_update_2d(const colvec& X1, const colvec& X2) +{ + mat R = yaw_to_R(X1(2)); + colvec X3(3); + X3.rows(0,1) = R * X2.rows(0,1) + X1.rows(0,1); + X3(2) = X1(2) + X2(2); + return X3; +} + +colvec pose_inverse_2d(const colvec& X) +{ + double c = cos(X(2)); + double s = sin(X(2)); + colvec XI(3); + XI(0) = -X(0) * c - X(1) * s; + XI(1) = X(0) * s - X(1) * c; + XI(2) = -X(2); + return XI; +} + + +// For Pose EKF ---------------------- + +mat Jplus1(const colvec& X1, const colvec& X2) +{ + colvec X3 = pose_update(X1,X2); + mat R1 = ypr_to_R(X1.rows(3,5)); + mat R2 = ypr_to_R(X2.rows(3,5)); + mat R3 = ypr_to_R(X3.rows(3,5)); + colvec o1 = R1.col(1); + colvec a1 = R1.col(2); + colvec o2 = R2.col(1); + colvec a2 = R2.col(2); + + mat I = eye(3,3); + mat Z = zeros(3,3); + + double Marr[9] = { -(X3(2-1)-X1(2-1)) , (X3(3-1)-X1(3-1))*cos(X1(4-1)) , a1(1-1)*X2(2-1)-o1(1-1)*X2(3-1) , + X3(1-1)-X1(1-1) , (X3(3-1)-X1(3-1))*sin(X1(4-1)) , a1(2-1)*X2(2-1)-o1(2-1)*X2(3-1) , + 0 , -X2(1-1)*cos(X1(5-1))-X2(2-1)*sin(X1(5-1))*sin(X1(6-1))-X2(3-1)*sin(X1(5-1))*cos(X1(6-1)) , a1(3-1)*X2(2-1)-o1(3-1)*X2(3-1) }; + mat M(3,3); + for (int i =0; i < 9; i++) + M(i) = Marr[i]; + M = trans(M); + + double K1arr[9] = { 1 , sin(X3(5-1))*sin(X3(4-1)-X1(4-1))/cos(X3(5-1)) , ( o2(1-1)*sin(X3(6-1))+a2(1-1)*cos(X3(6-1)) )/cos(X3(5-1)) , + 0 , cos(X3(4-1)-X1(4-1)) , -cos(X1(5-1))*sin(X3(4-1)-X1(4-1)) , + 0 , sin(X3(4-1)-X1(4-1))/cos(X3(5-1)) , cos(X1(5-1))*cos(X3(4-1)-X1(4-1))/cos(X3(5-1)) }; + mat K1(3,3); + for (int i =0; i < 9; i++) + K1(i) = K1arr[i]; + K1 = trans(K1); + + mat J1 = join_cols ( join_rows(I, M) , join_rows(Z,K1) ); + return J1; +} + +mat Jplus2(const colvec& X1, const colvec& X2) +{ + colvec X3 = pose_update(X1,X2); + mat R1 = ypr_to_R(X1.rows(3,5)); + mat R2 = ypr_to_R(X2.rows(3,5)); + mat R3 = ypr_to_R(X3.rows(3,5)); + colvec o1 = R1.col(1); + colvec a1 = R1.col(2); + colvec o2 = R2.col(1); + colvec a2 = R2.col(2); + + mat Z = zeros(3,3); + + double K2arr[9] = { cos(X2(5-1))*cos(X3(6-1)-X2(6-1))/cos(X3(5-1)) , sin(X3(6-1)-X2(6-1))/cos(X3(5-1)) , 0 , + -cos(X2(5-1))*sin(X3(6-1)-X2(6-1)) , cos(X3(6-1)-X2(6-1)) , 0 , + ( a1(1-1)*cos(X3(4-1))+a1(2-1)*sin(X3(4-1)) )/cos(X3(5-1)) , sin(X3(5-1))*sin(X3(6-1)-X2(6-1))/cos(X3(5-1)) , 1 }; + mat K2(3,3); + for (int i =0; i < 9; i++) + K2(i) = K2arr[i]; + K2 = trans(K2); + + mat J2 = join_cols ( join_rows(R1,Z) , join_rows(Z,K2) ); + return J2; +} + + +// For IMU EKF ---------------------- + +colvec state_update(const colvec& X, const colvec& U, double dt) +{ + double ro = X(3); + double pt = X(4); + double ya = X(5); + + colvec ypr(3); + ypr(0) = ya; + ypr(1) = pt; + ypr(2) = ro; + mat R = ypr_to_R(ypr); + + mat M(3,3); + M(0,0) = 1; M(0,1) = 0; M(0,2) = -sin(pt); + M(1,0) = 0; M(1,1) = cos(ro); M(1,2) = cos(pt)*sin(ro); + M(2,0) = 0; M(2,1) = -sin(ro); M(2,2) = cos(pt)*cos(ro); + + colvec Xt(9); + Xt.rows(0,2) = X.rows(0,2) + X.rows(6,8)*dt + R*U.rows(0,2)*dt*dt/2; + Xt.rows(3,5) = X.rows(3,5) + inv(M)*U.rows(3,5)*dt; + Xt.rows(6,8) = X.rows(6,8) + R*U.rows(0,2)*dt; + + return Xt; +} + +mat jacobianF(const colvec& X, const colvec& U, double dt) +{ + double x = X(0); + double y = X(1); + double z = X(2); + double ro = X(3); + double pt = X(4); + double ya = X(5); + double vx = X(6); + double vy = X(7); + double vz = X(8); + double ax = U(0); + double ay = U(1); + double az = U(2); + double wx = U(3); + double wy = U(4); + double wz = U(5); + + mat F(9,9); + + F(0,0) = 1; + F(0,1) = 0; + F(0,2) = 0; + F(0,3) = (dt*dt*(ay*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) + az*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro))))/2, + F(0,4) = (dt*dt*(az*cos(pt)*cos(ro)*cos(ya) - ax*cos(ya)*sin(pt) + ay*cos(pt)*cos(ya)*sin(ro)))/2; + F(0,5) = -(dt*dt*(ay*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)) - az*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + ax*cos(pt)*sin(ya)))/2; + F(0,6) = dt; + F(0,7) = 0; + F(0,8) = 0; + + F(1,0) = 0; + F(1,1) = 1; + F(1,2) = 0; + F(1,3) = -(dt*dt*(ay*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + az*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya))))/2; + F(1,4) = (dt*dt*(az*cos(pt)*cos(ro)*sin(ya) - ax*sin(pt)*sin(ya) + ay*cos(pt)*sin(ro)*sin(ya)))/2; + F(1,5) = (dt*dt*(az*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) - ay*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)) + ax*cos(pt)*cos(ya)))/2; + F(1,6) = 0; + F(1,7) = dt; + F(1,8) = 0; + + F(2,0) = 0; + F(2,1) = 0; + F(2,2) = 1; + F(2,3) = (dt*dt*(ay*cos(pt)*cos(ro) - az*cos(pt)*sin(ro)))/2, + F(2,4) = -(dt*dt*(ax*cos(pt) + az*cos(ro)*sin(pt) + ay*sin(pt)*sin(ro)))/2, + F(2,5) = 0; + F(2,6) = 0; + F(2,7) = 0; + F(2,8) = dt; + + F(3,0) = 0; + F(3,1) = 0; + F(3,2) = 0; + F(3,3) = 1 - dt*((wz*(cos(pt - ro)/2 - cos(pt + ro)/2))/cos(pt) - (wy*(sin(pt - ro)/2 + sin(pt + ro)/2))/cos(pt)); + F(3,4) = dt*((wz*(cos(pt - ro)/2 + cos(pt + ro)/2))/cos(pt) - (wy*(sin(pt - ro)/2 - sin(pt + ro)/2))/cos(pt) + + (wy*sin(pt)*(cos(pt - ro)/2 - cos(pt + ro)/2))/(cos(pt)*cos(pt)) + (wz*sin(pt)*(sin(pt - ro)/2 + sin(pt + ro)/2))/(cos(pt)*cos(pt))); + F(3,5) = 0; + F(3,6) = 0; + F(3,7) = 0; + F(3,8) = 0; + + F(4,0) = 0; + F(4,1) = 0; + F(4,2) = 0; + F(4,3) = -dt*(wz*cos(ro) + wy*sin(ro)); + F(4,4) = 1; + F(4,5) = 0; + F(4,6) = 0; + F(4,7) = 0; + F(4,8) = 0; + + F(5,0) = 0; + F(5,1) = 0; + F(5,2) = 0; + F(5,3) = dt*((wy*cos(ro))/cos(pt) - (wz*sin(ro))/cos(pt)); + F(5,4) = dt*((wz*cos(ro)*sin(pt))/(cos(pt)*cos(pt)) + (wy*sin(pt)*sin(ro))/(cos(pt)*cos(pt))); + F(5,5) = 1; + F(5,6) = 0; + F(5,7) = 0; + F(5,8) = 0; + + F(6,0) = 0; + F(6,1) = 0; + F(6,2) = 0; + F(6,3) = dt*(ay*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) + az*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro))); + F(6,4) = dt*(az*cos(pt)*cos(ro)*cos(ya) - ax*cos(ya)*sin(pt) + ay*cos(pt)*cos(ya)*sin(ro)); + F(6,5) = -dt*(ay*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)) - az*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + ax*cos(pt)*sin(ya)); + F(6,6) = 1; + F(6,7) = 0; + F(6,8) = 0; + + F(7,0) = 0; + F(7,1) = 0; + F(7,2) = 0; + F(7,3) = -dt*(ay*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + az*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya))); + F(7,4) = dt*(az*cos(pt)*cos(ro)*sin(ya) - ax*sin(pt)*sin(ya) + ay*cos(pt)*sin(ro)*sin(ya)); + F(7,5) = dt*(az*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) - ay*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)) + ax*cos(pt)*cos(ya)); + F(7,6) = 0; + F(7,7) = 1; + F(7,8) = 0; + + F(8,0) = 0; + F(8,1) = 0; + F(8,2) = 0; + F(8,3) = dt*(ay*cos(pt)*cos(ro) - az*cos(pt)*sin(ro)); + F(8,4) = -dt*(ax*cos(pt) + az*cos(ro)*sin(pt) + ay*sin(pt)*sin(ro)); + F(8,5) = 0; + F(8,6) = 0; + F(8,7) = 0; + F(8,8) = 1; + + return F; +} + +mat jacobianU(const colvec& X, const colvec& U, double dt) +{ + double x = X(0); + double y = X(1); + double z = X(2); + double ro = X(3); + double pt = X(4); + double ya = X(5); + double vx = X(6); + double vy = X(7); + double vz = X(8); + double ax = U(0); + double ay = U(1); + double az = U(2); + double wx = U(3); + double wy = U(4); + double wz = U(5); + + mat G(9,6); + + G(0,0) = (dt*dt*cos(pt)*cos(ya))/2; + G(0,1) = -(dt*dt*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)))/2; + G(0,2) = (dt*dt*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)))/2; + G(0,3) = 0; + G(0,4) = 0; + G(0,5) = 0; + + G(1,0) = (dt*dt*cos(pt)*sin(ya))/2; + G(1,1) = (dt*dt*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)))/2; + G(1,2) = -(dt*dt*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)))/2; + G(1,3) = 0; + G(1,4) = 0; + G(1,5) = 0; + + G(2,0) = -(dt*dt*sin(pt))/2; + G(2,1) = (dt*dt*cos(pt)*sin(ro))/2; + G(2,2) = (dt*dt*cos(pt)*cos(ro))/2; + G(2,3) = 0; + G(2,4) = 0; + G(2,5) = 0; + + G(3,0) = 0; + G(3,1) = 0; + G(3,2) = 0; + G(3,3) = dt; + G(3,4) = (dt*(cos(pt - ro)/2 - cos(pt + ro)/2))/cos(pt); + G(3,5) = (dt*(sin(pt - ro)/2 + sin(pt + ro)/2))/cos(pt); + + G(4,0) = 0; + G(4,1) = 0; + G(4,2) = 0; + G(4,3) = 0; + G(4,4) = dt*cos(ro); + G(4,5) = -dt*sin(ro); + + G(5,0) = 0; + G(5,1) = 0; + G(5,2) = 0; + G(5,3) = 0; + G(5,4) = (dt*sin(ro))/cos(pt); + G(5,5) = (dt*cos(ro))/cos(pt); + + G(6,0) = dt*cos(pt)*cos(ya); + G(6,1) = -dt*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)); + G(6,2) = dt*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)); + G(6,3) = 0; + G(6,4) = 0; + G(6,5) = 0; + + G(7,0) = dt*cos(pt)*sin(ya); + G(7,1) = dt*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)); + G(7,2) = -dt*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)); + G(7,3) = 0; + G(7,4) = 0; + G(7,5) = 0; + + G(8,0) = -dt*sin(pt); + G(8,1) = dt*cos(pt)*sin(ro); + G(8,2) = dt*cos(pt)*cos(ro); + G(8,3) = 0; + G(8,4) = 0; + G(8,5) = 0; + + return G; +} + +colvec state_measure(const colvec& X) +{ + colvec Z = X.rows(0,5); + return Z; +} + +mat jacobianH() +{ + mat H = zeros(6,9); + H.cols(0,5) = eye(6,6); + + return H; +} + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/src/pose_utils.cpp~ b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/src/pose_utils.cpp~ new file mode 100755 index 0000000..f2b4365 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/pose_utils/src/pose_utils.cpp~ @@ -0,0 +1,531 @@ +#include "pose_utils.h" + +// Rotation --------------------- + +mat ypr_to_R(const colvec& ypr) +{ + double c, s; + mat Rz = zeros(3,3); + double y = ypr(0); + c = cos(y); + s = sin(y); + Rz(0,0) = c; + Rz(1,0) = s; + Rz(0,1) = -s; + Rz(1,1) = c; + Rz(2,2) = 1; + + mat Ry = zeros(3,3); + double p = ypr(1); + c = cos(p); + s = sin(p); + Ry(0,0) = c; + Ry(2,0) = -s; + Ry(0,2) = s; + Ry(2,2) = c; + Ry(1,1) = 1; + + mat Rx = zeros(3,3); + double r = ypr(2); + c = cos(r); + s = sin(r); + Rx(1,1) = c; + Rx(2,1) = s; + Rx(1,2) = -s; + Rx(2,2) = c; + Rx(0,0) = 1; + + mat R = Rz*Ry*Rx; + return R; +} + +mat yaw_to_R(double yaw) +{ + mat R = zeros(2,2); + double c = cos(yaw); + double s = sin(yaw); + R(0,0) = c; + R(1,0) = s; + R(0,1) = -s; + R(1,1) = c; + return R; +} + +colvec R_to_ypr(const mat& R) +{ + colvec n = R.col(0); + colvec o = R.col(1); + colvec a = R.col(2); + + colvec ypr(3); + double y = atan2(n(1), n(0)); + double p = atan2(-n(2), n(0)*cos(y)+n(1)*sin(y)); + double r = atan2(a(0)*sin(y)-a(1)*cos(y), -o(0)*sin(y)+o(1)*cos(y)); + ypr(0) = y; + ypr(1) = p; + ypr(2) = r; + + return ypr; +} + +mat quaternion_to_R(const colvec& q) +{ + double n = norm(q, 2); + colvec nq = q / n; + + double w = nq(0); + double x = nq(1); + double y = nq(2); + double z = nq(3); + double w2 = w*w; + double x2 = x*x; + double y2 = y*y; + double z2 = z*z; + double xy = x*y; + double xz = x*z; + double yz = y*z; + double wx = w*x; + double wy = w*y; + double wz = w*z; + + mat R = zeros(3,3); + R(0,0) = w2+x2-y2-z2; + R(1,0) = 2*(wz + xy); + R(2,0) = 2*(xz - wy); + R(0,1) = 2*(xy - wz); + R(1,1) = w2-x2+y2-z2; + R(2,1) = 2*(wx + yz); + R(0,2) = 2*(wy + xz); + R(1,2) = 2*(yz - wx); + R(2,2) = w2-x2-y2+z2; + return R; +} + +colvec R_to_quaternion(const mat& R) +{ + colvec q(4); + double tr = R(0,0) + R(1,1) + R(2,2); + if (tr > 0) + { + double S = sqrt(tr + 1.0) * 2; + q(0) = 0.25 * S; + q(1) = (R(2,1) - R(1,2)) / S; + q(2) = (R(0,2) - R(2,0)) / S; + q(3) = (R(1,0) - R(0,1)) / S; + } + else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) + { + double S = sqrt(1.0 + R(0,0) - R(1,1) - R(2,2)) * 2; + q(0) = (R(2,1) - R(1,2)) / S; + q(1) = 0.25 * S; + q(2) = (R(0,1) + R(1,0)) / S; + q(3) = (R(0,2) + R(2,0)) / S; + } + else if (R(1,1) > R(2,2)) + { + double S = sqrt(1.0 + R(1,1) - R(0,0) - R(2,2)) * 2; + q(0) = (R(0,2) - R(2,0)) / S; + q(1) = (R(0,1) + R(1,0)) / S; + q(2) = 0.25 * S; + q(3) = (R(1,2) + R(2,1)) / S; + } + else + { + double S = sqrt(1.0 + R(2,2) - R(0,0) - R(1,1)) * 2; + q(0) = (R(1,0) - R(0,1)) / S; + q(1) = (R(0,2) + R(2,0)) / S; + q(2) = (R(1,2) + R(2,1)) / S; + q(3) = 0.25 * S; + } + return q; +} + +colvec quaternion_mul(const colvec& q1, const colvec& q2) +{ + double a1 = q1(0); + double b1 = q1(1); + double c1 = q1(2); + double d1 = q1(3); + + double a2 = q2(0); + double b2 = q2(1); + double c2 = q2(2); + double d2 = q2(3); + + colvec q3(4); + q3(0) = a1*a2 - b1*b2 - c1*c2 - d1*d2; + q3(1) = a1*b2 + b1*a2 + c1*d2 - d1*c2; + q3(2) = a1*c2 - b1*d2 + c1*a2 + d1*b2; + q3(3) = a1*d2 + b1*c2 - c1*b2 + d1*a2; + return q3; +} + +colvec quaternion_inv(const colvec& q) +{ + colvec q2(4); + q2(0) = q(0); + q2(1) = -q(1); + q2(2) = -q(2); + q2(3) = -q(3); + return q2; +} + +// General Pose Update ---------- + +colvec pose_update(const colvec& X1, const colvec& X2) +{ + mat R1 = ypr_to_R(X1.rows(3,5)); + mat R2 = ypr_to_R(X2.rows(3,5)); + mat R3 = R1 * R2; + + colvec X3xyz = X1.rows(0,2) + R1*X2.rows(0,2); + colvec X3ypr = R_to_ypr(R3); + + colvec X3 = join_cols(X3xyz, X3ypr); + return X3; +} + +colvec pose_inverse(const colvec& X) +{ + mat R = ypr_to_R(X.rows(3,5)); + colvec n = R.col(0); + colvec o = R.col(1); + colvec a = R.col(2); + + colvec XIxyz = -trans(R) * (X.rows(0,2)); + colvec XIypr(3); + double XIy = atan2(o(0), n(0)); + double XIp = atan2(-a(0), n(0)*cos(XIy)+o(0)*sin(XIy)); + double XIr = atan2(n(2)*sin(XIy)-o(2)*cos(XIy), -n(1)*sin(XIy)+o(1)*cos(XIy)); + XIypr(0) = XIy; + XIypr(1) = XIp; + XIypr(2) = XIr; + + colvec XI = join_cols(XIxyz, XIypr); + return XI; +} + +colvec pose_update_2d(const colvec& X1, const colvec& X2) +{ + mat R = yaw_to_R(X1(2)); + colvec X3(3); + X3.rows(0,1) = R * X2.rows(0,1) + X1.rows(0,1); + X3(2) = X1(2) + X2(2); + return X3; +} + +colvec pose_inverse_2d(const colvec& X) +{ + double c = cos(X(2)); + double s = sin(X(2)); + colvec XI(3); + XI(0) = -X(0) * c - X(1) * s; + XI(1) = X(0) * s - X(1) * c; + XI(2) = -X(2); + return XI; +} + + +// For Pose EKF ---------------------- + +mat Jplus1(const colvec& X1, const colvec& X2) +{ + colvec X3 = pose_update(X1,X2); + mat R1 = ypr_to_R(X1.rows(3,5)); + mat R2 = ypr_to_R(X2.rows(3,5)); + mat R3 = ypr_to_R(X3.rows(3,5)); + colvec o1 = R1.col(1); + colvec a1 = R1.col(2); + colvec o2 = R2.col(1); + colvec a2 = R2.col(2); + + mat I = eye(3,3); + mat Z = zeros(3,3); + + double Marr[9] = { -(X3(2-1)-X1(2-1)) , (X3(3-1)-X1(3-1))*cos(X1(4-1)) , a1(1-1)*X2(2-1)-o1(1-1)*X2(3-1) , + X3(1-1)-X1(1-1) , (X3(3-1)-X1(3-1))*sin(X1(4-1)) , a1(2-1)*X2(2-1)-o1(2-1)*X2(3-1) , + 0 , -X2(1-1)*cos(X1(5-1))-X2(2-1)*sin(X1(5-1))*sin(X1(6-1))-X2(3-1)*sin(X1(5-1))*cos(X1(6-1)) , a1(3-1)*X2(2-1)-o1(3-1)*X2(3-1) }; + mat M(3,3); + for (int i =0; i < 9; i++) + M(i) = Marr[i]; + M = trans(M); + + double K1arr[9] = { 1 , sin(X3(5-1))*sin(X3(4-1)-X1(4-1))/cos(X3(5-1)) , ( o2(1-1)*sin(X3(6-1))+a2(1-1)*cos(X3(6-1)) )/cos(X3(5-1)) , + 0 , cos(X3(4-1)-X1(4-1)) , -cos(X1(5-1))*sin(X3(4-1)-X1(4-1)) , + 0 , sin(X3(4-1)-X1(4-1))/cos(X3(5-1)) , cos(X1(5-1))*cos(X3(4-1)-X1(4-1))/cos(X3(5-1)) }; + mat K1(3,3); + for (int i =0; i < 9; i++) + K1(i) = K1arr[i]; + K1 = trans(K1); + + mat J1 = join_cols ( join_rows(I, M) , join_rows(Z,K1) ); + return J1; +} + +mat Jplus2(const colvec& X1, const colvec& X2) +{ + colvec X3 = pose_update(X1,X2); + mat R1 = ypr_to_R(X1.rows(3,5)); + mat R2 = ypr_to_R(X2.rows(3,5)); + mat R3 = ypr_to_R(X3.rows(3,5)); + colvec o1 = R1.col(1); + colvec a1 = R1.col(2); + colvec o2 = R2.col(1); + colvec a2 = R2.col(2); + + mat Z = zeros(3,3); + + double K2arr[9] = { cos(X2(5-1))*cos(X3(6-1)-X2(6-1))/cos(X3(5-1)) , sin(X3(6-1)-X2(6-1))/cos(X3(5-1)) , 0 , + -cos(X2(5-1))*sin(X3(6-1)-X2(6-1)) , cos(X3(6-1)-X2(6-1)) , 0 , + ( a1(1-1)*cos(X3(4-1))+a1(2-1)*sin(X3(4-1)) )/cos(X3(5-1)) , sin(X3(5-1))*sin(X3(6-1)-X2(6-1))/cos(X3(5-1)) , 1 }; + mat K2(3,3); + for (int i =0; i < 9; i++) + K2(i) = K2arr[i]; + K2 = trans(K2); + + mat J2 = join_cols ( join_rows(R1,Z) , join_rows(Z,K2) ); + return J2; +} + + +// For IMU EKF ---------------------- + +colvec state_update(const colvec& X, const colvec& U, double dt) +{ + double ro = X(3); + double pt = X(4); + double ya = X(5); + + colvec ypr(3); + ypr(0) = ya; + ypr(1) = pt; + ypr(2) = ro; + mat R = ypr_to_R(ypr); + + mat M(3,3); + M(0,0) = 1; M(0,1) = 0; M(0,2) = -sin(pt); + M(1,0) = 0; M(1,1) = cos(ro); M(1,2) = cos(pt)*sin(ro); + M(2,0) = 0; M(2,1) = -sin(ro); M(2,2) = cos(pt)*cos(ro); + + colvec Xt(9); + Xt.rows(0,2) = X.rows(0,2) + X.rows(6,8)*dt + R*U.rows(0,2)*dt*dt/2; + Xt.rows(3,5) = X.rows(3,5) + inv(M)*U.rows(3,5)*dt; + Xt.rows(6,8) = X.rows(6,8) + R*U.rows(0,2)*dt; + + return Xt; +} + +mat jacobianF(const colvec& X, const colvec& U, double dt) +{ + double x = X(0); + double y = X(1); + double z = X(2); + double ro = X(3); + double pt = X(4); + double ya = X(5); + double vx = X(6); + double vy = X(7); + double vz = X(8); + double ax = U(0); + double ay = U(1); + double az = U(2); + double wx = U(3); + double wy = U(4); + double wz = U(5); + + mat F(9,9); + + F(0,0) = 1; + F(0,1) = 0; + F(0,2) = 0; + F(0,3) = (dt*dt*(ay*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) + az*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro))))/2, + F(0,4) = (dt*dt*(az*cos(pt)*cos(ro)*cos(ya) - ax*cos(ya)*sin(pt) + ay*cos(pt)*cos(ya)*sin(ro)))/2; + F(0,5) = -(dt*dt*(ay*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)) - az*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + ax*cos(pt)*sin(ya)))/2; + F(0,6) = dt; + F(0,7) = 0; + F(0,8) = 0; + + F(1,0) = 0; + F(1,1) = 1; + F(1,2) = 0; + F(1,3) = -(dt*dt*(ay*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + az*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya))))/2; + F(1,4) = (dt*dt*(az*cos(pt)*cos(ro)*sin(ya) - ax*sin(pt)*sin(ya) + ay*cos(pt)*sin(ro)*sin(ya)))/2; + F(1,5) = (dt*dt*(az*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) - ay*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)) + ax*cos(pt)*cos(ya)))/2; + F(1,6) = 0; + F(1,7) = dt; + F(1,8) = 0; + + F(2,0) = 0; + F(2,1) = 0; + F(2,2) = 1; + F(2,3) = (dt*dt*(ay*cos(pt)*cos(ro) - az*cos(pt)*sin(ro)))/2, + F(2,4) = -(dt*dt*(ax*cos(pt) + az*cos(ro)*sin(pt) + ay*sin(pt)*sin(ro)))/2, + F(2,5) = 0; + F(2,6) = 0; + F(2,7) = 0; + F(2,8) = dt; + + F(3,0) = 0; + F(3,1) = 0; + F(3,2) = 0; + F(3,3) = 1 - dt*((wz*(cos(pt - ro)/2 - cos(pt + ro)/2))/cos(pt) - (wy*(sin(pt - ro)/2 + sin(pt + ro)/2))/cos(pt)); + F(3,4) = dt*((wz*(cos(pt - ro)/2 + cos(pt + ro)/2))/cos(pt) - (wy*(sin(pt - ro)/2 - sin(pt + ro)/2))/cos(pt) + + (wy*sin(pt)*(cos(pt - ro)/2 - cos(pt + ro)/2))/(cos(pt)*cos(pt)) + (wz*sin(pt)*(sin(pt - ro)/2 + sin(pt + ro)/2))/(cos(pt)*cos(pt))); + F(3,5) = 0; + F(3,6) = 0; + F(3,7) = 0; + F(3,8) = 0; + + F(4,0) = 0; + F(4,1) = 0; + F(4,2) = 0; + F(4,3) = -dt*(wz*cos(ro) + wy*sin(ro)); + F(4,4) = 1; + F(4,5) = 0; + F(4,6) = 0; + F(4,7) = 0; + F(4,8) = 0; + + F(5,0) = 0; + F(5,1) = 0; + F(5,2) = 0; + F(5,3) = dt*((wy*cos(ro))/cos(pt) - (wz*sin(ro))/cos(pt)); + F(5,4) = dt*((wz*cos(ro)*sin(pt))/(cos(pt)*cos(pt)) + (wy*sin(pt)*sin(ro))/(cos(pt)*cos(pt))); + F(5,5) = 1; + F(5,6) = 0; + F(5,7) = 0; + F(5,8) = 0; + + F(6,0) = 0; + F(6,1) = 0; + F(6,2) = 0; + F(6,3) = dt*(ay*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) + az*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro))); + F(6,4) = dt*(az*cos(pt)*cos(ro)*cos(ya) - ax*cos(ya)*sin(pt) + ay*cos(pt)*cos(ya)*sin(ro)); + F(6,5) = -dt*(ay*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)) - az*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + ax*cos(pt)*sin(ya)); + F(6,6) = 1; + F(6,7) = 0; + F(6,8) = 0; + + F(7,0) = 0; + F(7,1) = 0; + F(7,2) = 0; + F(7,3) = -dt*(ay*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)) + az*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya))); + F(7,4) = dt*(az*cos(pt)*cos(ro)*sin(ya) - ax*sin(pt)*sin(ya) + ay*cos(pt)*sin(ro)*sin(ya)); + F(7,5) = dt*(az*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)) - ay*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)) + ax*cos(pt)*cos(ya)); + F(7,6) = 0; + F(7,7) = 1; + F(7,8) = 0; + + F(8,0) = 0; + F(8,1) = 0; + F(8,2) = 0; + F(8,3) = dt*(ay*cos(pt)*cos(ro) - az*cos(pt)*sin(ro)); + F(8,4) = -dt*(ax*cos(pt) + az*cos(ro)*sin(pt) + ay*sin(pt)*sin(ro)); + F(8,5) = 0; + F(8,6) = 0; + F(8,7) = 0; + F(8,8) = 1; + + return F; +} + +mat jacobianU(const colvec& X, const colvec& U, double dt) +{ + double x = X(0); + double y = X(1); + double z = X(2); + double ro = X(3); + double pt = X(4); + double ya = X(5); + double vx = X(6); + double vy = X(7); + double vz = X(8); + double ax = U(0); + double ay = U(1); + double az = U(2); + double wx = U(3); + double wy = U(4); + double wz = U(5); + + mat G(9,6); + + G(0,0) = (dt*dt*cos(pt)*cos(ya))/2; + G(0,1) = -(dt*dt*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)))/2; + G(0,2) = (dt*dt*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)))/2; + G(0,3) = 0; + G(0,4) = 0; + G(0,5) = 0; + + G(1,0) = (dt*dt*cos(pt)*sin(ya))/2; + G(1,1) = (dt*dt*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)))/2; + G(1,2) = -(dt*dt*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)))/2; + G(1,3) = 0; + G(1,4) = 0; + G(1,5) = 0; + + G(2,0) = -(dt*dt*sin(pt))/2; + G(2,1) = (dt*dt*cos(pt)*sin(ro))/2; + G(2,2) = (dt*dt*cos(pt)*cos(ro))/2; + G(2,3) = 0; + G(2,4) = 0; + G(2,5) = 0; + + G(3,0) = 0; + G(3,1) = 0; + G(3,2) = 0; + G(3,3) = dt; + G(3,4) = (dt*(cos(pt - ro)/2 - cos(pt + ro)/2))/cos(pt); + G(3,5) = (dt*(sin(pt - ro)/2 + sin(pt + ro)/2))/cos(pt); + + G(4,0) = 0; + G(4,1) = 0; + G(4,2) = 0; + G(4,3) = 0; + G(4,4) = dt*cos(ro); + G(4,5) = -dt*sin(ro); + + G(5,0) = 0; + G(5,1) = 0; + G(5,2) = 0; + G(5,3) = 0; + G(5,4) = (dt*sin(ro))/cos(pt); + G(5,5) = (dt*cos(ro))/cos(pt); + + G(6,0) = dt*cos(pt)*cos(ya); + G(6,1) = -dt*(cos(ro)*sin(ya) - cos(ya)*sin(pt)*sin(ro)); + G(6,2) = dt*(sin(ro)*sin(ya) + cos(ro)*cos(ya)*sin(pt)); + G(6,3) = 0; + G(6,4) = 0; + G(6,5) = 0; + + G(7,0) = dt*cos(pt)*sin(ya); + G(7,1) = dt*(cos(ro)*cos(ya) + sin(pt)*sin(ro)*sin(ya)); + G(7,2) = -dt*(cos(ya)*sin(ro) - cos(ro)*sin(pt)*sin(ya)); + G(7,3) = 0; + G(7,4) = 0; + G(7,5) = 0; + + G(8,0) = -dt*sin(pt); + G(8,1) = dt*cos(pt)*sin(ro); + G(8,2) = dt*cos(pt)*cos(ro); + G(8,3) = 0; + G(8,4) = 0; + G(8,5) = 0; + + return G; +} + +colvec state_measure(const colvec& X) +{ + colvec Z = X.rows(0,5); + return Z; +} + +mat jacobianH() +{ + mat H = zeros(6,9); + H.cols(0,5) = eye(6,6); + + return H; +} + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/CMakeLists.txt new file mode 100755 index 0000000..2e446cf --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/CMakeLists.txt @@ -0,0 +1,109 @@ +#-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_library(encode_msgs src/encode_msgs.cpp) +#-rosbuild_add_link_flags(encode_msgs -Wl,--as-needed) + +#list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +#find_package(Eigen3 REQUIRED) + +#include_directories(${EIGEN3_INCLUDE_DIR}) +#-rosbuild_add_library(decode_msgs src/decode_msgs.cpp) +#-rosbuild_add_link_flags(decode_msgs -Wl,--as-needed) + +#------------------------------------------------------------------ +cmake_minimum_required(VERSION 2.8.3) +project(quadrotor_msgs) + +find_package(catkin REQUIRED COMPONENTS + #armadillo + roscpp + nav_msgs + geometry_msgs + message_generation +) + +set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) + +include_directories( + ${catkin_INCLUDE_DIRS} + include + ) + + +add_message_files( + FILES + AuxCommand.msg + Corrections.msg + Gains.msg + OutputData.msg + PositionCommand.msg + PPROutputData.msg + Serial.msg + SO3Command.msg + StatusData.msg + TRPYCommand.msg + Odometry.msg + PolynomialTrajectory.msg + LQRTrajectory.msg +) + +generate_messages( + DEPENDENCIES + geometry_msgs + nav_msgs +) + +catkin_package( + #INCLUDE_DIRS include + LIBRARIES encode_msgs decode_msgs + #CATKIN_DEPENDS geometry_msgs nav_msgs + #DEPENDS system_lib + CATKIN_DEPENDS message_runtime +) + +# add_executable(test_exe src/test_exe.cpp) +add_library(decode_msgs src/decode_msgs.cpp) +add_library(encode_msgs src/encode_msgs.cpp) + +# add_dependencies(test_exe quadrotor_msgs_generate_messages_cpp) +add_dependencies(encode_msgs quadrotor_msgs_generate_messages_cpp) +add_dependencies(decode_msgs quadrotor_msgs_generate_messages_cpp) + +find_package(Eigen3 REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIR}) + + +# target_link_libraries(test_exe +# decode_msgs +# encode_msgs +# ) + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake new file mode 100644 index 0000000..9c546a0 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/cmake/FindEigen3.cmake @@ -0,0 +1,81 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h new file mode 100755 index 0000000..42a08aa --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h @@ -0,0 +1,87 @@ +#ifndef __QUADROTOR_MSGS_COMM_TYPES_H__ +#define __QUADROTOR_MSGS_COMM_TYPES_H__ + +#define TYPE_SO3_CMD 's' +struct SO3_CMD_INPUT +{ + // Scaling factors when decoding + int16_t force[3]; // /500 + int8_t des_qx, des_qy, des_qz, des_qw; // /125 + uint8_t kR[3]; // /50 + uint8_t kOm[3]; // /100 + int16_t cur_yaw; // /1e4 + int16_t kf_correction; // /1e11; + uint8_t angle_corrections[2]; // roll,pitch /2500 + uint8_t enable_motors:1; + uint8_t use_external_yaw:1; + uint8_t seq; +}; + +#define TYPE_STATUS_DATA 'c' +struct STATUS_DATA +{ + uint16_t loop_rate; + uint16_t voltage; + uint8_t seq; +}; + +#define TYPE_OUTPUT_DATA 'd' +struct OUTPUT_DATA +{ + uint16_t loop_rate; + uint16_t voltage; + int16_t roll, pitch, yaw; + int16_t ang_vel[3]; + int16_t acc[3]; + int16_t dheight; + int32_t height; + int16_t mag[3]; + uint8_t radio[8]; + //uint8_t rpm[4]; + uint8_t seq; +}; + +#define TYPE_TRPY_CMD 'p' +struct TRPY_CMD +{ + int16_t thrust; + int16_t roll; + int16_t pitch; + int16_t yaw; + int16_t current_yaw; + uint8_t enable_motors:1; + uint8_t use_external_yaw:1; +}; + +#define TYPE_PPR_OUTPUT_DATA 't' +struct PPR_OUTPUT_DATA +{ + uint16_t time; + int16_t des_thrust; + int16_t des_roll; + int16_t des_pitch; + int16_t des_yaw; + int16_t est_roll; + int16_t est_pitch; + int16_t est_yaw; + int16_t est_angvel_x; + int16_t est_angvel_y; + int16_t est_angvel_z; + int16_t est_acc_x; + int16_t est_acc_y; + int16_t est_acc_z; + uint16_t pwm1; + uint16_t pwm2; + uint16_t pwm3; + uint16_t pwm4; +}; + +#define TYPE_PPR_GAINS 'g' +struct PPR_GAINS +{ + int16_t Kp; + int16_t Kd; + int16_t Kp_yaw; + int16_t Kd_yaw; +}; +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h new file mode 100755 index 0000000..a3076ea --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h @@ -0,0 +1,23 @@ +#ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ +#define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ + +#include +#include +#include +#include +#include + +namespace quadrotor_msgs +{ + +bool decodeOutputData(const std::vector &data, + quadrotor_msgs::OutputData &output); + +bool decodeStatusData(const std::vector &data, + quadrotor_msgs::StatusData &status); + +bool decodePPROutputData(const std::vector &data, + quadrotor_msgs::PPROutputData &output); +} + +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h new file mode 100755 index 0000000..a424b11 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h @@ -0,0 +1,22 @@ +#ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ +#define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__ + +#include +#include +#include +#include +#include + +namespace quadrotor_msgs +{ + +void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command, + std::vector &output); +void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command, + std::vector &output); + +void encodePPRGains(const quadrotor_msgs::Gains &gains, + std::vector &output); +} + +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg new file mode 100755 index 0000000..f59bf35 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/AuxCommand.msg @@ -0,0 +1,5 @@ +float64 current_yaw +float64 kf_correction +float64[2] angle_corrections# Trims for roll, pitch +bool enable_motors +bool use_external_yaw diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg new file mode 100755 index 0000000..e0f4e88 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Corrections.msg @@ -0,0 +1,2 @@ +float64 kf_correction +float64[2] angle_corrections diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg new file mode 100755 index 0000000..f5d10a3 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Gains.msg @@ -0,0 +1,4 @@ +float64 Kp +float64 Kd +float64 Kp_yaw +float64 Kd_yaw diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg new file mode 100644 index 0000000..0a34e9b --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/LQRTrajectory.msg @@ -0,0 +1,30 @@ +Header header + +# the trajectory id, starts from "1". +uint32 trajectory_id + +# the action command for trajectory server. +uint32 ACTION_ADD = 1 +uint32 ACTION_ABORT = 2 +uint32 ACTION_WARN_START = 3 +uint32 ACTION_WARN_FINAL = 4 +uint32 ACTION_WARN_IMPOSSIBLE = 5 +uint32 action + +# the weight coefficient of the control effort +float64 r + +# the yaw command +float64 start_yaw +float64 final_yaw + +# the initial and final state +float64[6] s0 +float64[3] ut + +float64[6] sf + +# the optimal arrival time +float64 t_f + +string debug_info diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg new file mode 100755 index 0000000..3272d71 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Odometry.msg @@ -0,0 +1,8 @@ +uint8 STATUS_ODOM_VALID=0 +uint8 STATUS_ODOM_INVALID=1 +uint8 STATUS_ODOM_LOOPCLOSURE=2 + +nav_msgs/Odometry curodom +nav_msgs/Odometry kfodom +uint32 kfid +uint8 status diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg new file mode 100755 index 0000000..ac95888 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/OutputData.msg @@ -0,0 +1,12 @@ +Header header +uint16 loop_rate +float64 voltage +geometry_msgs/Quaternion orientation +geometry_msgs/Vector3 angular_velocity +geometry_msgs/Vector3 linear_acceleration +float64 pressure_dheight +float64 pressure_height +geometry_msgs/Vector3 magnetic_field +uint8[8] radio_channel +#uint8[4] motor_rpm +uint8 seq diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg new file mode 100755 index 0000000..70434a0 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PPROutputData.msg @@ -0,0 +1,16 @@ +Header header +uint16 quad_time +float64 des_thrust +float64 des_roll +float64 des_pitch +float64 des_yaw +float64 est_roll +float64 est_pitch +float64 est_yaw +float64 est_angvel_x +float64 est_angvel_y +float64 est_angvel_z +float64 est_acc_x +float64 est_acc_y +float64 est_acc_z +uint16[4] pwm diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg new file mode 100755 index 0000000..0aab297 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg @@ -0,0 +1,28 @@ +Header header + +# the trajectory id, starts from "1". +uint32 trajectory_id + +# the action command for trajectory server. +uint32 ACTION_ADD = 1 +uint32 ACTION_ABORT = 2 +uint32 ACTION_WARN_START = 3 +uint32 ACTION_WARN_FINAL = 4 +uint32 ACTION_WARN_IMPOSSIBLE = 5 +uint32 action + +# the order of trajectory. +uint32 num_order +uint32 num_segment + +# the polynomial coecfficients of the trajectory. +float64 start_yaw +float64 final_yaw +float64[] coef_x +float64[] coef_y +float64[] coef_z +float64[] time +float64 mag_coeff +uint32[] order + +string debug_info diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg~ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg~ new file mode 100755 index 0000000..2112161 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PolynomialTrajectory.msg~ @@ -0,0 +1,26 @@ +Header header + +# the trajectory id, starts from "1". +uint32 trajectory_id + +# the action command for trajectory server. +uint32 ACTION_ADD = 1 +uint32 ACTION_ABORT = 2 +uint32 ACTION_WARN_START = 3 +uint32 ACTION_WARN_FINAL = 4 +uint32 ACTION_WARN_IMPOSSIBLE = 5 +uint32 action + +# the order of trajectory. +uint32 num_order +uint32 num_segment + +# the polynomial coecfficients of the trajectory. +float64 start_yaw +float64 final_yaw +float64[] coef_x +float64[] coef_y +float64[] coef_z +float64[] time +float64 mag_coeff +string debug_info diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg new file mode 100755 index 0000000..481518f --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg @@ -0,0 +1,21 @@ +Header header +geometry_msgs/Point position +geometry_msgs/Vector3 velocity +geometry_msgs/Vector3 acceleration +float64 yaw +float64 yaw_dot +float64[3] kx +float64[3] kv + +uint32 trajectory_id + +uint8 TRAJECTORY_STATUS_EMPTY = 0 +uint8 TRAJECTORY_STATUS_READY = 1 +uint8 TRAJECTORY_STATUS_COMPLETED = 3 +uint8 TRAJECTROY_STATUS_ABORT = 4 +uint8 TRAJECTORY_STATUS_ILLEGAL_START = 5 +uint8 TRAJECTORY_STATUS_ILLEGAL_FINAL = 6 +uint8 TRAJECTORY_STATUS_IMPOSSIBLE = 7 + +# Its ID number will start from 1, allowing you comparing it with 0. +uint8 trajectory_flag diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg~ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg~ new file mode 100755 index 0000000..4eda627 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/PositionCommand.msg~ @@ -0,0 +1,17 @@ +Header header +geometry_msgs/Point position +geometry_msgs/Vector3 velocity +geometry_msgs/Vector3 acceleration +float64 yaw +float64 yaw_dot +float64[3] kx +float64[3] kv + +uint32 trajectory_id + +uint8 TRAJECTORY_STATUS_EMPTY = 0 +uint8 TRAJECTORY_STATUS_READY = 1 +uint8 TRAJECTORY_STATUS_COMPLETE = 3 +uint8 TRAJECTROY_STATUS_ABORT = 8 + +uint8 trajectory_flag diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg new file mode 100755 index 0000000..d3868ef --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/SO3Command.msg @@ -0,0 +1,6 @@ +Header header +geometry_msgs/Vector3 force +geometry_msgs/Quaternion orientation +float64[3] kR +float64[3] kOm +quadrotor_msgs/AuxCommand aux diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg new file mode 100755 index 0000000..5a54cce --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/Serial.msg @@ -0,0 +1,13 @@ +# Note: These constants need to be kept in sync with the types +# defined in include/quadrotor_msgs/comm_types.h +uint8 SO3_CMD = 115 # 's' in base 10 +uint8 TRPY_CMD = 112 # 'p' in base 10 +uint8 STATUS_DATA = 99 # 'c' in base 10 +uint8 OUTPUT_DATA = 100 # 'd' in base 10 +uint8 PPR_OUTPUT_DATA = 116 # 't' in base 10 +uint8 PPR_GAINS = 103 # 'g' + +Header header +uint8 channel +uint8 type # One of the types listed above +uint8[] data diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg new file mode 100755 index 0000000..d176e4f --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/StatusData.msg @@ -0,0 +1,4 @@ +Header header +uint16 loop_rate +float64 voltage +uint8 seq diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg new file mode 100755 index 0000000..0d471a6 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/msg/TRPYCommand.msg @@ -0,0 +1,6 @@ +Header header +float32 thrust +float32 roll +float32 pitch +float32 yaw +quadrotor_msgs/AuxCommand aux diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/package.xml b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/package.xml new file mode 100755 index 0000000..42e9f76 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/package.xml @@ -0,0 +1,20 @@ + + quadrotor_msgs + 0.0.0 + quadrotor_msgs + Kartik Mohta + http://ros.org/wiki/quadrotor_msgs + BSD + catkin + geometry_msgs + nav_msgs + message_generation + geometry_msgs + nav_msgs + message_runtime + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp new file mode 100755 index 0000000..609ffb8 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/decode_msgs.cpp @@ -0,0 +1,103 @@ +#include "quadrotor_msgs/decode_msgs.h" +#include +#include + +namespace quadrotor_msgs +{ + +bool decodeOutputData(const std::vector &data, + quadrotor_msgs::OutputData &output) +{ + struct OUTPUT_DATA output_data; + if(data.size() != sizeof(output_data)) + return false; + + memcpy(&output_data, &data[0], sizeof(output_data)); + output.loop_rate = output_data.loop_rate; + output.voltage = output_data.voltage/1e3; + + const double roll = output_data.roll/1e2 * M_PI/180; + const double pitch = output_data.pitch/1e2 * M_PI/180; + const double yaw = output_data.yaw/1e2 * M_PI/180; + // Asctec (2012 firmware) uses Z-Y-X convention + Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()); + output.orientation.w = q.w(); + output.orientation.x = q.x(); + output.orientation.y = q.y(); + output.orientation.z = q.z(); + + output.angular_velocity.x = output_data.ang_vel[0]*0.0154*M_PI/180; + output.angular_velocity.y = output_data.ang_vel[1]*0.0154*M_PI/180; + output.angular_velocity.z = output_data.ang_vel[2]*0.0154*M_PI/180; + + output.linear_acceleration.x = output_data.acc[0]/1e3 * 9.81; + output.linear_acceleration.y = output_data.acc[1]/1e3 * 9.81; + output.linear_acceleration.z = output_data.acc[2]/1e3 * 9.81; + + output.pressure_dheight = output_data.dheight/1e3; + output.pressure_height = output_data.height/1e3; + + output.magnetic_field.x = output_data.mag[0]/2500.0; + output.magnetic_field.y = output_data.mag[1]/2500.0; + output.magnetic_field.z = output_data.mag[2]/2500.0; + + for(int i = 0; i < 8; i++) + { + output.radio_channel[i] = output_data.radio[i]; + } + //for(int i = 0; i < 4; i++) + // output.motor_rpm[i] = output_data.rpm[i]; + + output.seq = output_data.seq; + + return true; +} + +bool decodeStatusData(const std::vector &data, + quadrotor_msgs::StatusData &status) +{ + struct STATUS_DATA status_data; + if(data.size() != sizeof(status_data)) + return false; + memcpy(&status_data, &data[0], sizeof(status_data)); + + status.loop_rate = status_data.loop_rate; + status.voltage = status_data.voltage/1e3; + status.seq = status_data.seq; + + return true; +} + +bool decodePPROutputData(const std::vector &data, + quadrotor_msgs::PPROutputData &output) +{ + struct PPR_OUTPUT_DATA output_data; + if(data.size() != sizeof(output_data)) + return false; + memcpy(&output_data, &data[0], sizeof(output_data)); + + output.quad_time = output_data.time; + output.des_thrust = output_data.des_thrust*1e-4; + output.des_roll = output_data.des_roll*1e-4; + output.des_pitch = output_data.des_pitch*1e-4; + output.des_yaw = output_data.des_yaw*1e-4; + output.est_roll = output_data.est_roll*1e-4; + output.est_pitch = output_data.est_pitch*1e-4; + output.est_yaw = output_data.est_yaw*1e-4; + output.est_angvel_x = output_data.est_angvel_x*1e-3; + output.est_angvel_y = output_data.est_angvel_y*1e-3; + output.est_angvel_z = output_data.est_angvel_z*1e-3; + output.est_acc_x = output_data.est_acc_x*1e-4; + output.est_acc_y = output_data.est_acc_y*1e-4; + output.est_acc_z = output_data.est_acc_z*1e-4; + output.pwm[0] = output_data.pwm1; + output.pwm[1] = output_data.pwm2; + output.pwm[2] = output_data.pwm3; + output.pwm[3] = output_data.pwm4; + + return true; +} + +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp new file mode 100755 index 0000000..d0cccf2 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/encode_msgs.cpp @@ -0,0 +1,73 @@ +#include "quadrotor_msgs/encode_msgs.h" +#include + +namespace quadrotor_msgs +{ + +void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command, + std::vector &output) +{ + struct SO3_CMD_INPUT so3_cmd_input; + + so3_cmd_input.force[0] = so3_command.force.x*500; + so3_cmd_input.force[1] = so3_command.force.y*500; + so3_cmd_input.force[2] = so3_command.force.z*500; + + so3_cmd_input.des_qx = so3_command.orientation.x*125; + so3_cmd_input.des_qy = so3_command.orientation.y*125; + so3_cmd_input.des_qz = so3_command.orientation.z*125; + so3_cmd_input.des_qw = so3_command.orientation.w*125; + + so3_cmd_input.kR[0] = so3_command.kR[0]*50; + so3_cmd_input.kR[1] = so3_command.kR[1]*50; + so3_cmd_input.kR[2] = so3_command.kR[2]*50; + + so3_cmd_input.kOm[0] = so3_command.kOm[0]*100; + so3_cmd_input.kOm[1] = so3_command.kOm[1]*100; + so3_cmd_input.kOm[2] = so3_command.kOm[2]*100; + + so3_cmd_input.cur_yaw = so3_command.aux.current_yaw*1e4; + + so3_cmd_input.kf_correction = so3_command.aux.kf_correction*1e11; + so3_cmd_input.angle_corrections[0] = so3_command.aux.angle_corrections[0]*2500; + so3_cmd_input.angle_corrections[1] = so3_command.aux.angle_corrections[1]*2500; + + so3_cmd_input.enable_motors = so3_command.aux.enable_motors; + so3_cmd_input.use_external_yaw = so3_command.aux.use_external_yaw; + + so3_cmd_input.seq = so3_command.header.seq % 255; + + output.resize(sizeof(so3_cmd_input)); + memcpy(&output[0], &so3_cmd_input, sizeof(so3_cmd_input)); +} + +void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command, + std::vector &output) +{ + struct TRPY_CMD trpy_cmd_input; + trpy_cmd_input.thrust = trpy_command.thrust*1e4; + trpy_cmd_input.roll = trpy_command.roll*1e4; + trpy_cmd_input.pitch = trpy_command.pitch*1e4; + trpy_cmd_input.yaw = trpy_command.yaw*1e4; + trpy_cmd_input.current_yaw = trpy_command.aux.current_yaw*1e4; + trpy_cmd_input.enable_motors = trpy_command.aux.enable_motors; + trpy_cmd_input.use_external_yaw = trpy_command.aux.use_external_yaw; + + output.resize(sizeof(trpy_cmd_input)); + memcpy(&output[0], &trpy_cmd_input, sizeof(trpy_cmd_input)); +} + +void encodePPRGains(const quadrotor_msgs::Gains &gains, + std::vector &output) +{ + struct PPR_GAINS ppr_gains; + ppr_gains.Kp = gains.Kp; + ppr_gains.Kd = gains.Kd; + ppr_gains.Kp_yaw = gains.Kp_yaw; + ppr_gains.Kd_yaw = gains.Kd_yaw; + + output.resize(sizeof(ppr_gains)); + memcpy(&output[0], &ppr_gains, sizeof(ppr_gains)); +} + +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py new file mode 100755 index 0000000..652a255 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py @@ -0,0 +1,143 @@ +"""autogenerated by genpy from quadrotor_msgs/AuxCommand.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class AuxCommand(genpy.Message): + _md5sum = "94f75840e4b1e03675da764692f2c839" + _type = "quadrotor_msgs/AuxCommand" + _has_header = False #flag to mark the presence of a Header object + _full_text = """float64 current_yaw +float64 kf_correction +float64[2] angle_corrections# Trims for roll, pitch +bool enable_motors +bool use_external_yaw + +""" + __slots__ = ['current_yaw','kf_correction','angle_corrections','enable_motors','use_external_yaw'] + _slot_types = ['float64','float64','float64[2]','bool','bool'] + + 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: + current_yaw,kf_correction,angle_corrections,enable_motors,use_external_yaw + + :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(AuxCommand, self).__init__(*args, **kwds) + #message fields cannot be None, assign default values for those that are + if self.current_yaw is None: + self.current_yaw = 0. + if self.kf_correction is None: + self.kf_correction = 0. + if self.angle_corrections is None: + self.angle_corrections = [0.,0.] + if self.enable_motors is None: + self.enable_motors = False + if self.use_external_yaw is None: + self.use_external_yaw = False + else: + self.current_yaw = 0. + self.kf_correction = 0. + self.angle_corrections = [0.,0.] + self.enable_motors = False + self.use_external_yaw = False + + 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_2d.pack(_x.current_yaw, _x.kf_correction)) + buff.write(_struct_2d.pack(*self.angle_corrections)) + _x = self + buff.write(_struct_2B.pack(_x.enable_motors, _x.use_external_yaw)) + 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 += 16 + (_x.current_yaw, _x.kf_correction,) = _struct_2d.unpack(str[start:end]) + start = end + end += 16 + self.angle_corrections = _struct_2d.unpack(str[start:end]) + _x = self + start = end + end += 2 + (_x.enable_motors, _x.use_external_yaw,) = _struct_2B.unpack(str[start:end]) + self.enable_motors = bool(self.enable_motors) + self.use_external_yaw = bool(self.use_external_yaw) + 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_2d.pack(_x.current_yaw, _x.kf_correction)) + buff.write(self.angle_corrections.tostring()) + _x = self + buff.write(_struct_2B.pack(_x.enable_motors, _x.use_external_yaw)) + 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 += 16 + (_x.current_yaw, _x.kf_correction,) = _struct_2d.unpack(str[start:end]) + start = end + end += 16 + self.angle_corrections = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=2) + _x = self + start = end + end += 2 + (_x.enable_motors, _x.use_external_yaw,) = _struct_2B.unpack(str[start:end]) + self.enable_motors = bool(self.enable_motors) + self.use_external_yaw = bool(self.use_external_yaw) + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +_struct_2d = struct.Struct("<2d") +_struct_2B = struct.Struct("<2B") diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py new file mode 100755 index 0000000..d2b19c2 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py @@ -0,0 +1,111 @@ +"""autogenerated by genpy from quadrotor_msgs/Corrections.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class Corrections(genpy.Message): + _md5sum = "61e86887a75fe520847d3256306360f5" + _type = "quadrotor_msgs/Corrections" + _has_header = False #flag to mark the presence of a Header object + _full_text = """float64 kf_correction +float64[2] angle_corrections + +""" + __slots__ = ['kf_correction','angle_corrections'] + _slot_types = ['float64','float64[2]'] + + 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: + kf_correction,angle_corrections + + :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(Corrections, self).__init__(*args, **kwds) + #message fields cannot be None, assign default values for those that are + if self.kf_correction is None: + self.kf_correction = 0. + if self.angle_corrections is None: + self.angle_corrections = [0.,0.] + else: + self.kf_correction = 0. + self.angle_corrections = [0.,0.] + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + buff.write(_struct_d.pack(self.kf_correction)) + buff.write(_struct_2d.pack(*self.angle_corrections)) + 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 + start = end + end += 8 + (self.kf_correction,) = _struct_d.unpack(str[start:end]) + start = end + end += 16 + self.angle_corrections = _struct_2d.unpack(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: + buff.write(_struct_d.pack(self.kf_correction)) + buff.write(self.angle_corrections.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 + start = end + end += 8 + (self.kf_correction,) = _struct_d.unpack(str[start:end]) + start = end + end += 16 + self.angle_corrections = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=2) + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +_struct_2d = struct.Struct("<2d") +_struct_d = struct.Struct(" 0x03000000 else False +import genpy +import struct + + +class Gains(genpy.Message): + _md5sum = "b82763b9f24e5595e2a816aa779c9461" + _type = "quadrotor_msgs/Gains" + _has_header = False #flag to mark the presence of a Header object + _full_text = """float64 Kp +float64 Kd +float64 Kp_yaw +float64 Kd_yaw + +""" + __slots__ = ['Kp','Kd','Kp_yaw','Kd_yaw'] + _slot_types = ['float64','float64','float64','float64'] + + 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: + Kp,Kd,Kp_yaw,Kd_yaw + + :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(Gains, self).__init__(*args, **kwds) + #message fields cannot be None, assign default values for those that are + if self.Kp is None: + self.Kp = 0. + if self.Kd is None: + self.Kd = 0. + if self.Kp_yaw is None: + self.Kp_yaw = 0. + if self.Kd_yaw is None: + self.Kd_yaw = 0. + else: + self.Kp = 0. + self.Kd = 0. + self.Kp_yaw = 0. + self.Kd_yaw = 0. + + 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_4d.pack(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw)) + 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 += 32 + (_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw,) = _struct_4d.unpack(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_4d.pack(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw)) + 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 += 32 + (_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw,) = _struct_4d.unpack(str[start:end]) + 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") diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py new file mode 100755 index 0000000..357c5f1 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py @@ -0,0 +1,277 @@ +"""autogenerated by genpy from quadrotor_msgs/OutputData.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + +import geometry_msgs.msg +import std_msgs.msg + +class OutputData(genpy.Message): + _md5sum = "5759ee97fd5c090dcdccf7fc3e50d024" + _type = "quadrotor_msgs/OutputData" + _has_header = True #flag to mark the presence of a Header object + _full_text = """Header header +uint16 loop_rate +float64 voltage +geometry_msgs/Quaternion orientation +geometry_msgs/Vector3 angular_velocity +geometry_msgs/Vector3 linear_acceleration +float64 pressure_dheight +float64 pressure_height +geometry_msgs/Vector3 magnetic_field +uint8[8] radio_channel +#uint8[4] motor_rpm +uint8 seq + +================================================================================ +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: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. + +float64 x +float64 y +float64 z +""" + __slots__ = ['header','loop_rate','voltage','orientation','angular_velocity','linear_acceleration','pressure_dheight','pressure_height','magnetic_field','radio_channel','seq'] + _slot_types = ['std_msgs/Header','uint16','float64','geometry_msgs/Quaternion','geometry_msgs/Vector3','geometry_msgs/Vector3','float64','float64','geometry_msgs/Vector3','uint8[8]','uint8'] + + 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,loop_rate,voltage,orientation,angular_velocity,linear_acceleration,pressure_dheight,pressure_height,magnetic_field,radio_channel,seq + + :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(OutputData, 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.loop_rate is None: + self.loop_rate = 0 + if self.voltage is None: + self.voltage = 0. + if self.orientation is None: + self.orientation = geometry_msgs.msg.Quaternion() + if self.angular_velocity is None: + self.angular_velocity = geometry_msgs.msg.Vector3() + if self.linear_acceleration is None: + self.linear_acceleration = geometry_msgs.msg.Vector3() + if self.pressure_dheight is None: + self.pressure_dheight = 0. + if self.pressure_height is None: + self.pressure_height = 0. + if self.magnetic_field is None: + self.magnetic_field = geometry_msgs.msg.Vector3() + if self.radio_channel is None: + self.radio_channel = chr(0)*8 + if self.seq is None: + self.seq = 0 + else: + self.header = std_msgs.msg.Header() + self.loop_rate = 0 + self.voltage = 0. + self.orientation = geometry_msgs.msg.Quaternion() + self.angular_velocity = geometry_msgs.msg.Vector3() + self.linear_acceleration = geometry_msgs.msg.Vector3() + self.pressure_dheight = 0. + self.pressure_height = 0. + self.magnetic_field = geometry_msgs.msg.Vector3() + self.radio_channel = chr(0)*8 + self.seq = 0 + + 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(' 0x03000000 else False +import genpy +import struct + +import std_msgs.msg + +class PPROutputData(genpy.Message): + _md5sum = "732c0e3ca36f241464f8c445e78a0d0a" + _type = "quadrotor_msgs/PPROutputData" + _has_header = True #flag to mark the presence of a Header object + _full_text = """Header header +uint16 quad_time +float64 des_thrust +float64 des_roll +float64 des_pitch +float64 des_yaw +float64 est_roll +float64 est_pitch +float64 est_yaw +float64 est_angvel_x +float64 est_angvel_y +float64 est_angvel_z +float64 est_acc_x +float64 est_acc_y +float64 est_acc_z +uint16[4] pwm + +================================================================================ +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 + +""" + __slots__ = ['header','quad_time','des_thrust','des_roll','des_pitch','des_yaw','est_roll','est_pitch','est_yaw','est_angvel_x','est_angvel_y','est_angvel_z','est_acc_x','est_acc_y','est_acc_z','pwm'] + _slot_types = ['std_msgs/Header','uint16','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','uint16[4]'] + + 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,quad_time,des_thrust,des_roll,des_pitch,des_yaw,est_roll,est_pitch,est_yaw,est_angvel_x,est_angvel_y,est_angvel_z,est_acc_x,est_acc_y,est_acc_z,pwm + + :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(PPROutputData, 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.quad_time is None: + self.quad_time = 0 + if self.des_thrust is None: + self.des_thrust = 0. + if self.des_roll is None: + self.des_roll = 0. + if self.des_pitch is None: + self.des_pitch = 0. + if self.des_yaw is None: + self.des_yaw = 0. + if self.est_roll is None: + self.est_roll = 0. + if self.est_pitch is None: + self.est_pitch = 0. + if self.est_yaw is None: + self.est_yaw = 0. + if self.est_angvel_x is None: + self.est_angvel_x = 0. + if self.est_angvel_y is None: + self.est_angvel_y = 0. + if self.est_angvel_z is None: + self.est_angvel_z = 0. + if self.est_acc_x is None: + self.est_acc_x = 0. + if self.est_acc_y is None: + self.est_acc_y = 0. + if self.est_acc_z is None: + self.est_acc_z = 0. + if self.pwm is None: + self.pwm = [0,0,0,0] + else: + self.header = std_msgs.msg.Header() + self.quad_time = 0 + self.des_thrust = 0. + self.des_roll = 0. + self.des_pitch = 0. + self.des_yaw = 0. + self.est_roll = 0. + self.est_pitch = 0. + self.est_yaw = 0. + self.est_angvel_x = 0. + self.est_angvel_y = 0. + self.est_angvel_z = 0. + self.est_acc_x = 0. + self.est_acc_y = 0. + self.est_acc_z = 0. + self.pwm = [0,0,0,0] + + 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(' 0x03000000 else False +import genpy +import struct + +import geometry_msgs.msg +import std_msgs.msg + +class PositionCommand(genpy.Message): + _md5sum = "835935bcd6f18632d9e26a3093237902" + _type = "quadrotor_msgs/PositionCommand" + _has_header = True #flag to mark the presence of a Header object + _full_text = """Header header +geometry_msgs/Point position +geometry_msgs/Vector3 velocity +geometry_msgs/Vector3 acceleration +float64 yaw +float64 yaw_dot +float64[3] kx +float64[3] kv + +================================================================================ +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: geometry_msgs/Point +# This contains the position of a point in free space +float64 x +float64 y +float64 z + +================================================================================ +MSG: geometry_msgs/Vector3 +# This represents a vector in free space. + +float64 x +float64 y +float64 z +""" + __slots__ = ['header','position','velocity','acceleration','yaw','yaw_dot','kx','kv'] + _slot_types = ['std_msgs/Header','geometry_msgs/Point','geometry_msgs/Vector3','geometry_msgs/Vector3','float64','float64','float64[3]','float64[3]'] + + 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,position,velocity,acceleration,yaw,yaw_dot,kx,kv + + :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(PositionCommand, 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.position is None: + self.position = geometry_msgs.msg.Point() + if self.velocity is None: + self.velocity = geometry_msgs.msg.Vector3() + if self.acceleration is None: + self.acceleration = geometry_msgs.msg.Vector3() + if self.yaw is None: + self.yaw = 0. + if self.yaw_dot is None: + self.yaw_dot = 0. + if self.kx is None: + self.kx = [0.,0.,0.] + if self.kv is None: + self.kv = [0.,0.,0.] + else: + self.header = std_msgs.msg.Header() + self.position = geometry_msgs.msg.Point() + self.velocity = geometry_msgs.msg.Vector3() + self.acceleration = geometry_msgs.msg.Vector3() + self.yaw = 0. + self.yaw_dot = 0. + self.kx = [0.,0.,0.] + self.kv = [0.,0.,0.] + + 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(' 0x03000000 else False +import genpy +import struct + +import geometry_msgs.msg +import quadrotor_msgs.msg +import std_msgs.msg + +class SO3Command(genpy.Message): + _md5sum = "a466650b2633e768513aa3bf62383c86" + _type = "quadrotor_msgs/SO3Command" + _has_header = True #flag to mark the presence of a Header object + _full_text = """Header header +geometry_msgs/Vector3 force +geometry_msgs/Quaternion orientation +float64[3] kR +float64[3] kOm +quadrotor_msgs/AuxCommand aux + +================================================================================ +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: geometry_msgs/Vector3 +# This represents a vector 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: quadrotor_msgs/AuxCommand +float64 current_yaw +float64 kf_correction +float64[2] angle_corrections# Trims for roll, pitch +bool enable_motors +bool use_external_yaw + +""" + __slots__ = ['header','force','orientation','kR','kOm','aux'] + _slot_types = ['std_msgs/Header','geometry_msgs/Vector3','geometry_msgs/Quaternion','float64[3]','float64[3]','quadrotor_msgs/AuxCommand'] + + 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,force,orientation,kR,kOm,aux + + :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(SO3Command, 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.force is None: + self.force = geometry_msgs.msg.Vector3() + if self.orientation is None: + self.orientation = geometry_msgs.msg.Quaternion() + if self.kR is None: + self.kR = [0.,0.,0.] + if self.kOm is None: + self.kOm = [0.,0.,0.] + if self.aux is None: + self.aux = quadrotor_msgs.msg.AuxCommand() + else: + self.header = std_msgs.msg.Header() + self.force = geometry_msgs.msg.Vector3() + self.orientation = geometry_msgs.msg.Quaternion() + self.kR = [0.,0.,0.] + self.kOm = [0.,0.,0.] + self.aux = quadrotor_msgs.msg.AuxCommand() + + 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(' 0x03000000 else False +import genpy +import struct + +import std_msgs.msg + +class Serial(genpy.Message): + _md5sum = "e448fb7595af9a8adfcab5ec241c7d4f" + _type = "quadrotor_msgs/Serial" + _has_header = True #flag to mark the presence of a Header object + _full_text = """# Note: These constants need to be kept in sync with the types +# defined in include/quadrotor_msgs/comm_types.h +uint8 SO3_CMD = 115 # 's' in base 10 +uint8 TRPY_CMD = 112 # 'p' in base 10 +uint8 STATUS_DATA = 99 # 'c' in base 10 +uint8 OUTPUT_DATA = 100 # 'd' in base 10 +uint8 PPR_OUTPUT_DATA = 116 # 't' in base 10 +uint8 PPR_GAINS = 103 # 'g' + +Header header +uint8 channel +uint8 type # One of the types listed above +uint8[] 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 + +""" + # Pseudo-constants + SO3_CMD = 115 + TRPY_CMD = 112 + STATUS_DATA = 99 + OUTPUT_DATA = 100 + PPR_OUTPUT_DATA = 116 + PPR_GAINS = 103 + + __slots__ = ['header','channel','type','data'] + _slot_types = ['std_msgs/Header','uint8','uint8','uint8[]'] + + 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,channel,type,data + + :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(Serial, 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.channel is None: + self.channel = 0 + if self.type is None: + self.type = 0 + if self.data is None: + self.data = '' + else: + self.header = std_msgs.msg.Header() + self.channel = 0 + self.type = 0 + self.data = '' + + 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(' 0x03000000 else False +import genpy +import struct + +import std_msgs.msg + +class StatusData(genpy.Message): + _md5sum = "c70a4ec4725273263ae176ad30f89553" + _type = "quadrotor_msgs/StatusData" + _has_header = True #flag to mark the presence of a Header object + _full_text = """Header header +uint16 loop_rate +float64 voltage +uint8 seq + +================================================================================ +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 + +""" + __slots__ = ['header','loop_rate','voltage','seq'] + _slot_types = ['std_msgs/Header','uint16','float64','uint8'] + + 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,loop_rate,voltage,seq + + :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(StatusData, 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.loop_rate is None: + self.loop_rate = 0 + if self.voltage is None: + self.voltage = 0. + if self.seq is None: + self.seq = 0 + else: + self.header = std_msgs.msg.Header() + self.loop_rate = 0 + self.voltage = 0. + self.seq = 0 + + 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(' 0x03000000 else False +import genpy +import struct + +import quadrotor_msgs.msg +import std_msgs.msg + +class TRPYCommand(genpy.Message): + _md5sum = "6779ee8a86cc757aeea21725df32d00c" + _type = "quadrotor_msgs/TRPYCommand" + _has_header = True #flag to mark the presence of a Header object + _full_text = """Header header +float32 thrust +float32 roll +float32 pitch +float32 yaw +quadrotor_msgs/AuxCommand aux + +================================================================================ +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: quadrotor_msgs/AuxCommand +float64 current_yaw +float64 kf_correction +float64[2] angle_corrections# Trims for roll, pitch +bool enable_motors +bool use_external_yaw + +""" + __slots__ = ['header','thrust','roll','pitch','yaw','aux'] + _slot_types = ['std_msgs/Header','float32','float32','float32','float32','quadrotor_msgs/AuxCommand'] + + 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,thrust,roll,pitch,yaw,aux + + :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(TRPYCommand, 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.thrust is None: + self.thrust = 0. + if self.roll is None: + self.roll = 0. + if self.pitch is None: + self.pitch = 0. + if self.yaw is None: + self.yaw = 0. + if self.aux is None: + self.aux = quadrotor_msgs.msg.AuxCommand() + else: + self.header = std_msgs.msg.Header() + self.thrust = 0. + self.roll = 0. + self.pitch = 0. + self.yaw = 0. + self.aux = quadrotor_msgs.msg.AuxCommand() + + 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(' + Value: true + - Class: rviz/Axes + Enabled: true + Length: 10 + Name: Axes + Radius: 0.01 + Reference Frame: + Value: true + Enabled: true + Name: Basic + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf/robot + Name: Robot UKF + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose UKF + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /odom_visualization_ukf/pose + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf/velocity + Name: Velocity UKF + Namespaces: + velocity: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path UKF + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /odom_visualization_ukf/path + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /odom_visualization_ukf/covariance + Name: Cov UKF + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: true + Name: UKF Odom + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_slam/robot + Name: Robot SLAM + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 0.3 + Axes Radius: 0.12 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose SLAM + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /odom_visualization_slam/pose + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_slam/velocity + Name: Velocity SLAM + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 255; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path SLAM + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /odom_visualization_slam/path + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /odom_visualization_slam/covariance + Name: Cov SLAM + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: false + Name: SLAM Odom + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path World + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /waypoint_generator/waypoints_vis + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 255; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path Desired + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /trajectory_generator/path + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 255 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path Trajectory + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /trajectory_generator/traj + Value: true + Enabled: true + Name: Trajectory Generator + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 255; 0; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Features + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.2 + Style: Spheres + Topic: /visual_pose_estimator/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 255; 255; 255 + Color Transformer: RGBF32 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Loc Err Points + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Spheres + Topic: /visual_pose_estimator/loc_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /visual_pose_estimator/loc_connect + Name: Loc Err Lines + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 255; 255; 255 + Color Transformer: RGBF32 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Stereo Dense + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Boxes + Topic: /visual_slam/cloud_stereo + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Stereo Sparse + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Spheres + Topic: /visual_slam/points_stereo + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.2 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.8 + Min Value: -0.8 + Value: true + Axis: Z + Channel Name: z + Class: rviz/PointCloud + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: -0.8 + Name: Map 3D Vision + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.2 + Style: Boxes + Topic: /visual_slam/map + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /visual_slam/pose_graph_odom + Name: SLAM Pose Graph + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /visual_slam/pose_graph_loop + Name: SLAM Loop CLosure + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /visual_slam/pose_graph_loop_tf + Name: SLAM Loop TF + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: true + Name: Hummingbird + - Class: rviz/Group + Displays: + - Alpha: 0.7 + Class: rviz_plugins/ProbMapDisplay + Draw Behind: false + Enabled: true + Name: Map 2D Laser + Topic: /map3d_visualization/map2d + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 255; 255; 255 + Color Transformer: RGBF32 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Map 3D Laser + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Spheres + Topic: /map3d_visualization/map3d + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.60212 + Min Value: -0.161681 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 0; 4 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 0; 4 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Laser Raw + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /laser/scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 0 + Max Intensity: 4096 + Min Color: 255; 255; 0 + Min Intensity: 0 + Name: Laser + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /laser_odom_pelican/lscan + Use Fixed Frame: true + Use rainbow: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 255; 170; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Kinect + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /kinect/depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 255; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: RRT* Path + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /rrts/path + Value: true + - Arrow Length: 0.3 + Class: rviz/PoseArray + Color: 255; 255; 0 + Enabled: true + Name: RRT* Pose + Topic: /rrts/path_pose + Value: true + Enabled: true + Name: Pelican + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /monocular_velocity_estimator/plane + Name: Plane + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /monocular_velocity_estimator/normal + Name: Normal + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /monocular_velocity_estimator/velocity + Name: Velocity + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: OpticalFlow + - Class: rviz/Group + Displays: + - Alpha: 0.7 + Class: rviz_plugins/AerialMapDisplay + Draw Behind: true + Enabled: true + Name: AerialMapDisplay + Topic: /aerial_map_publisher/aerial_map + Value: true + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf_laser/robot + Name: Robot Laser + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose Laser + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /odom_visualization_ukf_laser/pose + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf_laser/velocity + Name: Velocity Laser + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path Laser + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /odom_visualization_ukf_laser/path + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /odom_visualization_ukf_laser/covariance + Name: Cov Laser + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: false + Name: Laser + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf_vision/robot + Name: Robot Vision + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose Vision + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /odom_visualization_ukf_vision/pose + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /odom_visualization_ukf_vision/velocity + Name: Velocity Vision + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 255; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path Vision + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /odom_visualization_ukf_vision/path + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /odom_visualization_ukf_vision/covariance + Name: Cov Vision + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Stereo Features + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 10 + Size (m): 0.2 + Style: Points + Topic: /stereo_pose_estimator/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Vision + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf_gps/robot + Name: Robot GPS + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose GPS + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /odom_visualization_ukf_gps/pose + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf_gps/velocity + Name: Velocity GPS + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 255; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path GPS + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /odom_visualization_ukf_gps/path + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf_gps/covariance + Name: Cov GPS + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: false + Name: GPS + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf_multi/robot + Name: Robot Multi + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose Multi + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /odom_visualization_ukf_multi/pose + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf_multi/velocity + Name: Velocity Multi + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path Multi + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /odom_visualization_ukf_multi/path + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /odom_visualization_ukf_multi/trajectory + Name: Traj Multi + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf_multi/covariance + Name: Cov Multi + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /odom_visualization_ukf_multi/covariance_velocity + Name: Cov Vel Multi + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /multi_sensor_slam/graph + Name: SLAM Graph + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz_plugins/MultiProbMapDisplay + Draw Behind: true + Enabled: true + Name: MultiProbMapDisplay + Topic: /multi_map_visualization/maps2d + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.15 + Min Value: -0.35 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Multi3DMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Spheres + Topic: /multi_map_visualization/map3d + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.5 + Buffer Length: 1 + Class: rviz/Range + Color: 255; 0; 255 + Enabled: true + Name: Laser Height + Queue Size: 100 + Topic: /odom_visualization_ukf_multi/height + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_ukf_multi/sensor + Name: Sensor Availability + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: Multi + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_vins/robot + Name: Robot VINS + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose VINS + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /odom_visualization_vins/pose + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_vins/velocity + Name: Velocity VINS + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: false + Line Style: Lines + Line Width: 0.03 + Name: Path VINS + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /odom_visualization_vins/path + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Cloud VINS + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Spheres + Topic: /monocular_vins_estimator/cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Arrow Length: 0.3 + Class: rviz/PoseArray + Color: 255; 0; 0 + Enabled: true + Name: Poses VINS + Topic: /monocular_vins_estimator/poses + Value: true + Enabled: true + Name: VINS + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_nonlinear/robot + Name: Robot Nonlinear + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose Nonlinear + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /odom_visualization_nonlinear/pose + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_nonlinear/velocity + Name: Velocity Nonlinear + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 255; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /odom_visualization_nonlinear/path + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Cloud Nonlinear + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Spheres + Topic: /monocular_vins_estimator/cloud_nonlinear + Use Fixed Frame: true + Use rainbow: true + Value: true + - Arrow Length: 0.3 + Class: rviz/PoseArray + Color: 0; 255; 0 + Enabled: true + Name: Poses Nonlinear + Topic: /monocular_vins_estimator/poses_nonlinear + Value: true + Enabled: true + Name: Nonlinear + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_imu/robot + Name: Robot IMU + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: false + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose IMU + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /odom_visualization_imu/pose + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_imu/velocity + Name: Velocity IMU + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 255; 0 + Enabled: false + Line Style: Lines + Line Width: 0.03 + Name: Path IMU + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /odom_visualization_imu/path + Value: false + Enabled: false + Name: IMU + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_control/robot + Name: Robot Control + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 0.75 + Axes Radius: 0.075 + Class: rviz/Pose + Color: 0; 0; 255 + Enabled: false + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose Control + Shaft Length: 0.5 + Shaft Radius: 0.05 + Shape: Axes + Topic: /odom_visualization_control/pose + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /odom_visualization_control/velocity + Name: Velocity Control + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: false + Line Style: Lines + Line Width: 0.03 + Name: Path Control + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /odom_visualization_control/path + Value: false + Enabled: false + Name: Control + Enabled: true + Name: VINS + Enabled: true + Name: NUC + - Class: rviz/Marker + Enabled: true + Marker Topic: /irobot/irobot_marker + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 61; 61; 61 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/MoveCamera + - Class: rviz_plugins/Goal3DTool + Topic: goal + - Class: rviz/SetInitialPose + Topic: trigger + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 0.0262036 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 4.92202 + Y: -11.2494 + Z: 11.5579 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.674797 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.72857 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 744 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000010000000000000190000002a2fc020000000ffb0000000a0049006d0061006700650000000041000001760000000000000000fb000000100044006900730070006c0061007900730100000028000002a2000000dd00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001c5000000810000006400fffffffb0000000a0049006d00610067006501000001fc0000011d0000000000000000fb0000000a0049006d0061006700650100000154000000e90000000000000000fb0000000a0049006d0061006700650100000211000001080000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000041000004f40000006400fffffffb000000100044006900730070006c0061007900730100000041000002d80000000000000000fb0000000a0049006d00610067006501000001db0000013e0000000000000000fb0000000a0049006d0061006700650100000186000001930000000000000000fb0000000a00560069006500770073000000017c0000019d000000b000fffffffb0000000a0049006d00610067006501000001da0000013f0000000000000000fb0000000a0049006d006100670065010000027d0000009c0000000000000000fb0000000a0049006d00610067006501000001d2000001470000000000000000fb0000000a0049006d00610067006501000001af0000016a000000000000000000000383000002a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1305 + X: 61 + Y: 24 diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/package.xml b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/package.xml new file mode 100644 index 0000000..af4b617 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/package.xml @@ -0,0 +1,33 @@ + + rviz_plugins + + + Additional plugins for rviz + + + 0.1.0 + Shaojie Shen + william.wu + LGPLv3 + http://ros.org/wiki/rviz_plugins + + catkin + + rviz + roscpp + multi_map_server + qtbase5-dev + message_runtime + + rviz + multi_map_server + message_runtime + roscpp + libqt5-core + libqt5-gui + libqt5-widgets + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/plugin_description.xml b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/plugin_description.xml new file mode 100755 index 0000000..26acf43 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/plugin_description.xml @@ -0,0 +1,42 @@ + + + + Tool for setting 3D goal + + + + + + Display of -inf +inf probabilistic occupancy grid map + + + + + + game usage + + + + + + Display of aerial map + + + + + + Display of multiple -inf +inf probabilistic occupancy grid map + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.cpp new file mode 100755 index 0000000..8ef2441 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.cpp @@ -0,0 +1,573 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the Willow Garage, 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. + */ + +#include + +#include +#include +#include +#include +#include + +#include + +#include + +#include "rviz/display_context.h" +#include "rviz/frame_manager.h" +#include "rviz/ogre_helpers/grid.h" +#include "rviz/properties/float_property.h" +#include "rviz/properties/int_property.h" +#include "rviz/properties/property.h" +#include "rviz/properties/quaternion_property.h" +#include "rviz/properties/ros_topic_property.h" +#include "rviz/properties/vector_property.h" +#include "rviz/validate_floats.h" + +#include "aerialmap_display.h" + +namespace rviz +{ + +AerialMapDisplay::AerialMapDisplay() + : Display() + , manual_object_(NULL) + //! @bug cannot compile @gcc-5 or later, material_(0) + , loaded_(false) + , resolution_(0.0f) + , width_(0) + , height_(0) + , position_(Ogre::Vector3::ZERO) + , orientation_(Ogre::Quaternion::IDENTITY) + , new_map_(false) +{ + topic_property_ = new RosTopicProperty( + "Topic", "", QString::fromStdString( + ros::message_traits::datatype()), + "nav_msgs::OccupancyGrid topic to subscribe to.", this, + SLOT(updateTopic())); + + alpha_property_ = new FloatProperty( + "Alpha", 0.7, "Amount of transparency to apply to the map.", this, + SLOT(updateAlpha())); + alpha_property_->setMin(0); + alpha_property_->setMax(1); + + draw_under_property_ = + new Property("Draw Behind", false, + "Rendering option, controls whether or not the map is always" + " drawn behind everything else.", + this, SLOT(updateDrawUnder())); + + resolution_property_ = new FloatProperty( + "Resolution", 0, "Resolution of the map. (not editable)", this); + resolution_property_->setReadOnly(true); + + width_property_ = new IntProperty( + "Width", 0, "Width of the map, in meters. (not editable)", this); + width_property_->setReadOnly(true); + + height_property_ = new IntProperty( + "Height", 0, "Height of the map, in meters. (not editable)", this); + height_property_->setReadOnly(true); + + position_property_ = new VectorProperty( + "Position", Ogre::Vector3::ZERO, + "Position of the bottom left corner of the map, in meters. (not editable)", + this); + position_property_->setReadOnly(true); + + orientation_property_ = + new QuaternionProperty("Orientation", Ogre::Quaternion::IDENTITY, + "Orientation of the map. (not editable)", this); + orientation_property_->setReadOnly(true); +} + +AerialMapDisplay::~AerialMapDisplay() +{ + unsubscribe(); + clear(); +} + +void +AerialMapDisplay::onInitialize() +{ + static int count = 0; + std::stringstream ss; + ss << "AerialMapObjectMaterial" << count++; + material_ = Ogre::MaterialManager::getSingleton().create( + ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material_->setReceiveShadows(false); + material_->getTechnique(0)->setLightingEnabled(false); + material_->setDepthBias(-16.0f, 0.0f); + material_->setCullingMode(Ogre::CULL_NONE); + material_->setDepthWriteEnabled(false); + + updateAlpha(); +} + +void +AerialMapDisplay::onEnable() +{ + subscribe(); +} + +void +AerialMapDisplay::onDisable() +{ + unsubscribe(); + clear(); +} + +void +AerialMapDisplay::subscribe() +{ + if (!isEnabled()) + { + return; + } + + if (!topic_property_->getTopic().isEmpty()) + { + try + { + map_sub_ = + update_nh_.subscribe(topic_property_->getTopicStd(), 1, + &AerialMapDisplay::incomingAerialMap, this); + setStatus(StatusProperty::Ok, "Topic", "OK"); + } + catch (ros::Exception& e) + { + setStatus(StatusProperty::Error, "Topic", + QString("Error subscribing: ") + e.what()); + } + } +} + +void +AerialMapDisplay::unsubscribe() +{ + map_sub_.shutdown(); +} + +void +AerialMapDisplay::updateAlpha() +{ + float alpha = alpha_property_->getFloat(); + + Ogre::Pass* pass = material_->getTechnique(0)->getPass(0); + Ogre::TextureUnitState* tex_unit = NULL; + if (pass->getNumTextureUnitStates() > 0) + { + tex_unit = pass->getTextureUnitState(0); + } + else + { + tex_unit = pass->createTextureUnitState(); + } + + tex_unit->setAlphaOperation(Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, + Ogre::LBS_CURRENT, alpha); + + if (alpha < 0.9998) + { + material_->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material_->setDepthWriteEnabled(false); + } + else + { + material_->setSceneBlending(Ogre::SBT_REPLACE); + material_->setDepthWriteEnabled(!draw_under_property_->getValue().toBool()); + } +} + +void +AerialMapDisplay::updateDrawUnder() +{ + bool draw_under = draw_under_property_->getValue().toBool(); + + if (alpha_property_->getFloat() >= 0.9998) + { + material_->setDepthWriteEnabled(!draw_under); + } + + if (manual_object_) + { + if (draw_under) + { + manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4); + } + else + { + manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_MAIN); + } + } +} + +void +AerialMapDisplay::updateTopic() +{ + unsubscribe(); + subscribe(); + clear(); +} + +void +AerialMapDisplay::clear() +{ + setStatus(StatusProperty::Warn, "Message", "No map received"); + + if (!loaded_) + { + return; + } + + scene_manager_->destroyManualObject(manual_object_); + manual_object_ = NULL; + + std::string tex_name = texture_->getName(); + texture_.setNull(); + Ogre::TextureManager::getSingleton().unload(tex_name); + + loaded_ = false; +} +/* +bool validateFloats(const nav_msgs::OccupancyGrid& msg) +{ + bool valid = true; + valid = valid && validateFloats( msg.info.resolution ); + valid = valid && validateFloats( msg.info.origin ); + return valid; +} +*/ +void +AerialMapDisplay::update(float wall_dt, float ros_dt) +{ + { + boost::mutex::scoped_lock lock(mutex_); + + current_map_ = updated_map_; + } + + if (!current_map_ || !new_map_) + { + return; + } + + if (current_map_->data.empty()) + { + return; + } + + new_map_ = false; + /* + if( !validateFloats( *current_map_ )) + { + setStatus( StatusProperty::Error, "Map", "Message contained invalid + floating point values (nans or infs)" ); + return; + } + */ + if (current_map_->info.width * current_map_->info.height == 0) + { + std::stringstream ss; + ss << "AerialMap is zero-sized (" << current_map_->info.width << "x" + << current_map_->info.height << ")"; + setStatus(StatusProperty::Error, "AerialMap", + QString::fromStdString(ss.str())); + return; + } + + clear(); + + setStatus(StatusProperty::Ok, "Message", "AerialMap received"); + + ROS_DEBUG("Received a %d X %d map @ %.3f m/pix\n", current_map_->info.width, + current_map_->info.height, current_map_->info.resolution); + + float resolution = current_map_->info.resolution; + + int width = current_map_->info.width; + int height = current_map_->info.height; + + Ogre::Vector3 position(current_map_->info.origin.position.x, + current_map_->info.origin.position.y, + current_map_->info.origin.position.z); + Ogre::Quaternion orientation(current_map_->info.origin.orientation.w, + current_map_->info.origin.orientation.x, + current_map_->info.origin.orientation.y, + current_map_->info.origin.orientation.z); + frame_ = current_map_->header.frame_id; + if (frame_.empty()) + { + frame_ = "/map"; + } + + // Expand it to be RGB data + unsigned int pixels_size = width * height * 3; + unsigned char* pixels = new unsigned char[pixels_size]; + memset(pixels, 255, pixels_size); + + bool map_status_set = false; + unsigned int num_pixels_to_copy = pixels_size; + if (pixels_size != current_map_->data.size()) + { + std::stringstream ss; + ss << "Data size doesn't match width*height: width = " << width + << ", height = " << height + << ", data size = " << current_map_->data.size(); + setStatus(StatusProperty::Error, "AerialMap", + QString::fromStdString(ss.str())); + map_status_set = true; + + // Keep going, but don't read past the end of the data. + if (current_map_->data.size() < pixels_size) + { + num_pixels_to_copy = current_map_->data.size(); + } + } + + // TODO: a fragment shader could do this on the video card, and + // would allow a non-grayscale color to mark the out-of-range + // values. + for (unsigned int pixel_index = 0; pixel_index < num_pixels_to_copy; + pixel_index++) + { + /* + unsigned char val; + int8_t data = current_map_->data[ pixel_index ]; + if(data > 0) + val = 0; + else if(data < 0) + val = 255; + else + val = 127; + pixels[ pixel_index ] = val; + */ + pixels[pixel_index] = current_map_->data[pixel_index]; + } + + Ogre::DataStreamPtr pixel_stream; + pixel_stream.bind(new Ogre::MemoryDataStream(pixels, pixels_size)); + static int tex_count = 0; + std::stringstream ss; + ss << "AerialMapTexture" << tex_count++; + try + { + texture_ = Ogre::TextureManager::getSingleton().loadRawData( + ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + pixel_stream, width, height, Ogre::PF_R8G8B8, Ogre::TEX_TYPE_2D, 0); + + if (!map_status_set) + { + setStatus(StatusProperty::Ok, "AerialMap", "AerialMap OK"); + } + } + catch (Ogre::RenderingAPIException&) + { + Ogre::Image image; + pixel_stream->seek(0); + float fwidth = width; + float fheight = height; + if (width > height) + { + float aspect = fheight / fwidth; + fwidth = 2048; + fheight = fwidth * aspect; + } + else + { + float aspect = fwidth / fheight; + fheight = 2048; + fwidth = fheight * aspect; + } + + { + std::stringstream ss; + ss << "AerialMap is larger than your graphics card supports. " + "Downsampled from [" + << width << "x" << height << "] to [" << fwidth << "x" << fheight + << "]"; + setStatus(StatusProperty::Ok, "AerialMap", + QString::fromStdString(ss.str())); + } + + ROS_WARN("Failed to create full-size map texture, likely because your " + "graphics card does not support textures of size > 2048. " + "Downsampling to [%d x %d]...", + (int)fwidth, (int)fheight); + // ROS_INFO("Stream size [%d], width [%f], height [%f], w * h [%f]", + // pixel_stream->size(), width, height, width * height); + image.loadRawData(pixel_stream, width, height, Ogre::PF_R8G8B8); + image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST); + ss << "Downsampled"; + texture_ = Ogre::TextureManager::getSingleton().loadImage( + ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image); + } + + delete[] pixels; + + Ogre::Pass* pass = material_->getTechnique(0)->getPass(0); + Ogre::TextureUnitState* tex_unit = NULL; + if (pass->getNumTextureUnitStates() > 0) + { + tex_unit = pass->getTextureUnitState(0); + } + else + { + tex_unit = pass->createTextureUnitState(); + } + + tex_unit->setTextureName(texture_->getName()); + tex_unit->setTextureFiltering(Ogre::TFO_NONE); + + static int map_count = 0; + std::stringstream ss2; + ss2 << "AerialMapObject" << map_count++; + manual_object_ = scene_manager_->createManualObject(ss2.str()); + scene_node_->attachObject(manual_object_); + + manual_object_->begin(material_->getName(), + Ogre::RenderOperation::OT_TRIANGLE_LIST); + { + // First triangle + { + // Bottom left + manual_object_->position(0.0f, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 0.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Top right + manual_object_->position(resolution * width, resolution * height, 0.0f); + manual_object_->textureCoord(1.0f, 1.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Top left + manual_object_->position(0.0f, resolution * height, 0.0f); + manual_object_->textureCoord(0.0f, 1.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + } + + // Second triangle + { + // Bottom left + manual_object_->position(0.0f, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 0.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Bottom right + manual_object_->position(resolution * width, 0.0f, 0.0f); + manual_object_->textureCoord(1.0f, 0.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Top right + manual_object_->position(resolution * width, resolution * height, 0.0f); + manual_object_->textureCoord(1.0f, 1.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + } + } + manual_object_->end(); + + if (draw_under_property_->getValue().toBool()) + { + manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4); + } + + resolution_property_->setValue(resolution); + width_property_->setValue(width); + height_property_->setValue(height); + position_property_->setVector(position); + orientation_property_->setQuaternion(orientation); + + transformAerialMap(); + + loaded_ = true; + + context_->queueRender(); +} + +void +AerialMapDisplay::incomingAerialMap( + const nav_msgs::OccupancyGrid::ConstPtr& msg) +{ + + updated_map_ = msg; + boost::mutex::scoped_lock lock(mutex_); + new_map_ = true; +} + +void +AerialMapDisplay::transformAerialMap() +{ + if (!current_map_) + { + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->transform( + frame_, ros::Time(), current_map_->info.origin, position, orientation)) + { + ROS_DEBUG("Error transforming map '%s' from frame '%s' to frame '%s'", + qPrintable(getName()), frame_.c_str(), qPrintable(fixed_frame_)); + + setStatus(StatusProperty::Error, "Transform", + "No transform from [" + QString::fromStdString(frame_) + + "] to [" + fixed_frame_ + "]"); + } + else + { + setStatus(StatusProperty::Ok, "Transform", "Transform OK"); + } + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); +} + +void +AerialMapDisplay::fixedFrameChanged() +{ + transformAerialMap(); +} + +void +AerialMapDisplay::reset() +{ + Display::reset(); + + clear(); + // Force resubscription so that the map will be re-sent + updateTopic(); +} + +} // namespace rviz + +#include +PLUGINLIB_EXPORT_CLASS(rviz::AerialMapDisplay, rviz::Display) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.h new file mode 100755 index 0000000..72504f1 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/aerialmap_display.h @@ -0,0 +1,134 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * 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 the Willow Garage, 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. + */ + +#ifndef AERIAL_MAP_DISPLAY_H +#define AERIAL_MAP_DISPLAY_H + +#include +#include +#include + +#include +#include + +#include + +#include "rviz/display.h" + +namespace Ogre +{ +class ManualObject; +} + +namespace rviz +{ + +class FloatProperty; +class IntProperty; +class Property; +class QuaternionProperty; +class RosTopicProperty; +class VectorProperty; + +/** + * \class AerialMapDisplay + * \brief Displays a map along the XY plane. + */ +class AerialMapDisplay: public Display +{ +Q_OBJECT +public: + AerialMapDisplay(); + virtual ~AerialMapDisplay(); + + // Overrides from Display + virtual void onInitialize(); + virtual void fixedFrameChanged(); + virtual void reset(); + virtual void update( float wall_dt, float ros_dt ); + + float getResolution() { return resolution_; } + int getWidth() { return width_; } + int getHeight() { return height_; } + Ogre::Vector3 getPosition() { return position_; } + Ogre::Quaternion getOrientation() { return orientation_; } + +protected Q_SLOTS: + void updateAlpha(); + void updateTopic(); + void updateDrawUnder(); + + +protected: + // overrides from Display + virtual void onEnable(); + virtual void onDisable(); + + virtual void subscribe(); + virtual void unsubscribe(); + + void incomingAerialMap(const nav_msgs::OccupancyGrid::ConstPtr& msg); + + void clear(); + + void transformAerialMap(); + + Ogre::ManualObject* manual_object_; + Ogre::TexturePtr texture_; + Ogre::MaterialPtr material_; + bool loaded_; + + std::string topic_; + float resolution_; + int width_; + int height_; + Ogre::Vector3 position_; + Ogre::Quaternion orientation_; + std::string frame_; + + ros::Subscriber map_sub_; + + RosTopicProperty* topic_property_; + FloatProperty* resolution_property_; + IntProperty* width_property_; + IntProperty* height_property_; + VectorProperty* position_property_; + QuaternionProperty* orientation_property_; + FloatProperty* alpha_property_; + Property* draw_under_property_; + + nav_msgs::OccupancyGrid::ConstPtr updated_map_; + nav_msgs::OccupancyGrid::ConstPtr current_map_; + boost::mutex mutex_; + bool new_map_; +}; + +} // namespace rviz + + #endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/gamelikeinput.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/gamelikeinput.cpp new file mode 100644 index 0000000..b30730a --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/gamelikeinput.cpp @@ -0,0 +1,433 @@ +// cheat the compiler + +#include +#include + +#include "rviz/selection/forwards.h" +#include "rviz/selection/selection_manager.h" + +#define private public +#include "rviz/default_plugin/markers/marker_selection_handler.h" +#include "rviz/viewport_mouse_event.h" +#undef private + +// #include "gamelikeinput.hpp" + +#include "rviz/default_plugin/tools/move_tool.h" +#include "rviz/display_context.h" +#include "rviz/render_panel.h" +#include "rviz/tool_manager.h" + +#include "boost/unordered_map.hpp" + +#include +#include +#include +#include + +#include "rviz/geometry.h" +#include "rviz/load_resource.h" +#include "rviz/ogre_helpers/arrow.h" +#include "rviz/render_panel.h" +#include "rviz/viewport_mouse_event.h" + +#include +#include + +#include "rviz/properties/string_property.h" + +#include "nav_msgs/Path.h" +// #include "quadrotor_msgs/SwarmCommand.h" +#include "std_msgs/Int32MultiArray.h" + +//! @todo rewrite this grabage code + +void +GameLikeInput::updateTopic() +{ + pub_pointlist = + nh_.advertise(topic_property_wp_->getStdString(), 1); + + pub_selection = nh_.advertise( + topic_property_drone_->getStdString(), 1); + + pub_swarm = nh_.advertise( + topic_property_swarm_->getStdString(), 1); + + z_max = property_z_max->getFloat(); + z_min = property_z_min->getFloat(); + y_max = property_y_max->getFloat(); + y_min = property_y_min->getFloat(); + x_max = property_x_max->getFloat(); + x_min = property_x_min->getFloat(); +} + +GameLikeInput::GameLikeInput() + : rviz::SelectionTool() + , move_tool_(new rviz::InteractionTool()) + , selecting_(false) + , sel_start_x_(0) + , sel_start_y_(0) + , moving_(false) +{ + shortcut_key_ = 'z'; + access_all_keys_ = true; + + topic_property_wp_ = + new rviz::StringProperty("TopicPoint", "point_list", + "The topic on which to publish navigation goals.", + getPropertyContainer(), SLOT(updateTopic()), this); + + topic_property_drone_ = + new rviz::StringProperty("TopicSelect", "select_list", + "The topic on which to publish select drone id.", + getPropertyContainer(), SLOT(updateTopic()), this); + + topic_property_swarm_ = new rviz::StringProperty( + "TopicSwarm", "swarm", "The topic on which to publish swarm command.", + getPropertyContainer(), SLOT(updateTopic()), this); + + property_z_max = new rviz::FloatProperty( + "RangeZ_max", 1.7, "", getPropertyContainer(), SLOT(updateTopic()), this); + property_z_min = new rviz::FloatProperty( + "RangeZ_min", 0.8, "", getPropertyContainer(), SLOT(updateTopic()), this); + + property_y_max = new rviz::FloatProperty( + "RangeY_max", 2.5, "", getPropertyContainer(), SLOT(updateTopic()), this); + property_y_min = new rviz::FloatProperty( + "RangeY_min", -2.5, "", getPropertyContainer(), SLOT(updateTopic()), this); + + property_x_max = new rviz::FloatProperty( + "RangeX_max", 4, "", getPropertyContainer(), SLOT(updateTopic()), this); + property_x_min = new rviz::FloatProperty( + "RangeX_min", -4, "", getPropertyContainer(), SLOT(updateTopic()), this); +} + +GameLikeInput::~GameLikeInput() +{ + delete move_tool_; + delete topic_property_wp_; + delete topic_property_drone_; + delete topic_property_swarm_; + + delete property_z_max; + delete property_z_min; + delete property_x_max; + delete property_x_min; + delete property_y_max; + delete property_y_min; +} + +void +GameLikeInput::onInitialize() +{ + this->move_tool_->initialize(context_); + + arrow_ = new rviz::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f); + arrow_->setColor(0.3f, 0.35f, 0.9f, 1.0f); + arrow_->getSceneNode()->setVisible(false); + + state_ = None; + updateTopic(); +} + +void +GameLikeInput::sendMessage() +{ + std_msgs::Int32MultiArray array; + nav_msgs::Path path; + + array.data.clear(); + + rviz::SelectionManager* sel_manager = context_->getSelectionManager(); + + auto size = std::distance(selection_.begin(), selection_.end()); + + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.position.z = 0; + + //! @note makeup selection array + if (!selection_.empty()) + { + for (auto it = selection_.begin(); it != selection_.end(); it++) + { + rviz::Picked p = it->second; + rviz::SelectionHandler* sh = sel_manager->getHandler(p.handle); + rviz::MarkerSelectionHandler* mh = + reinterpret_cast(sh); + + QString cpy = mh->marker_id_; + + //! @note use the mean position of every selected drone as start point + pose.pose.position.x += mh->getPosition().x; + pose.pose.position.y += mh->getPosition().y; + pose.pose.position.z += mh->getPosition().z; + + array.data.push_back(cpy.remove("drone/").toInt()); + } + + pose.pose.position.x /= size; + pose.pose.position.y /= size; + pose.pose.position.z /= size; + + path.poses.push_back(pose); + } + + //! @note go through arrow_array, get the internal parameter of each array + //! makeup each waypoint + + for (int i = 0; i < arrow_array.size(); ++i) + { + rviz::Arrow* p_a = arrow_array[i]; + geometry_msgs::PoseStamped ps; + + ps.pose.position.x = p_a->getPosition().x; + ps.pose.position.y = p_a->getPosition().y; + ps.pose.position.z = z_vector[i]; + + if (ps.pose.position.z > z_max) + ps.pose.position.z = z_max; + if (ps.pose.position.z < z_min) + ps.pose.position.z = z_min; + + if (ps.pose.position.y > y_max) + ps.pose.position.y = y_max; + if (ps.pose.position.y < y_min) + ps.pose.position.y = y_min; + + if (ps.pose.position.x > x_max) + ps.pose.position.x = x_max; + if (ps.pose.position.x < x_min) + ps.pose.position.x = x_min; + + path.poses.push_back(ps); + + delete p_a; + } + z_vector.clear(); + arrow_array.clear(); + arrow_->getSceneNode()->setVisible(false); + + path.header.frame_id = "map"; + path.header.stamp = ros::Time::now(); + + std::sort(array.data.begin(), array.data.end()); + + quadrotor_msgs::SwarmCommand swarm; + swarm.plan = path; + swarm.selection = array.data; + + pub_selection.publish(array); + pub_pointlist.publish(path); + pub_swarm.publish(swarm); +} + +void +GameLikeInput::onPoseSet(double x, double y, double z, double theta) +{ + ROS_WARN("3D Goal Set"); + std::string fixed_frame = context_->getFixedFrame().toStdString(); + tf::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + tf::Stamped p = tf::Stamped( + tf::Pose(quat, tf::Point(x, y, z)), ros::Time::now(), fixed_frame); + geometry_msgs::PoseStamped goal; + tf::poseStampedTFToMsg(p, goal); + ROS_INFO("Setting goal: Frame:%s, Position(%.3f, %.3f, %.3f), " + "Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", + fixed_frame.c_str(), goal.pose.position.x, goal.pose.position.y, + goal.pose.position.z, goal.pose.orientation.x, + goal.pose.orientation.y, goal.pose.orientation.z, + goal.pose.orientation.w, theta); +} + +int +GameLikeInput::processMouseEvent(rviz::ViewportMouseEvent& event) +{ + + rviz::SelectionManager* sel_manager = context_->getSelectionManager(); + + int flags = 0; + + // wp vars + static double initz; + static double prevz; + static double prevangle; + const double z_scale = 50; + Ogre::Quaternion orient_x = + Ogre::Quaternion(Ogre::Radian(-Ogre::Math::PI), Ogre::Vector3::UNIT_X); + + if (event.alt()) + { + moving_ = true; + selecting_ = false; + } + else + { + moving_ = false; + + if (event.leftDown()) + { + this->selecting_ = true; + + this->sel_start_x_ = event.x; + this->sel_start_y_ = event.y; + } + } + + if (selecting_) + { + rviz::M_Picked selection; + + sel_manager->highlight(event.viewport, sel_start_x_, sel_start_y_, event.x, + event.y); + + if (event.leftUp()) + { + rviz::SelectionManager::SelectType type = rviz::SelectionManager::Replace; + + if (event.shift()) + { + type = rviz::SelectionManager::Add; + } + else if (event.control()) + { + type = rviz::SelectionManager::Remove; + } + + // boost::recursive_mutex::scoped_lock lock(global_mutex_); + sel_manager->select(event.viewport, this->sel_start_x_, + this->sel_start_y_, event.x, event.y, type); + selection = sel_manager->getSelection(); + + rviz::MarkerSelectionHandler* cmp = new rviz::MarkerSelectionHandler( + nullptr, std::pair(), context_); + + for (auto it = selection.begin(); it != selection.end();) + { + rviz::Picked p = it->second; + rviz::SelectionHandler* sh = sel_manager->getHandler(p.handle); + void (rviz::SelectionHandler::*vptr)(const rviz::Picked& obj, + rviz::Property* parent_property) = + &rviz::SelectionHandler::createProperties; + // black magic, must have warning + bool e = false; + if (reinterpret_cast(sh->*vptr) != + reinterpret_cast(cmp->*vptr)) + e = true; + else + { + rviz::MarkerSelectionHandler* mh = + reinterpret_cast(sh); + if (mh->marker_id_.startsWith("drone")) + e = false; + else + e = true; + } + + if (e) + it = selection.erase(it); + else + it++; + } + delete cmp; + + sel_manager->setSelection(selection); + selection_ = selection; + + selecting_ = false; + } + + flags |= Render; + } + else if (moving_) + { + sel_manager->removeHighlight(); + flags = move_tool_->processMouseEvent(event); + + if (event.type == QEvent::MouseButtonRelease) + { + moving_ = false; + } + } + else + { + + if (event.rightDown()) + { + if (state_ == None) + { + Ogre::Vector3 intersection; + Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); + if (rviz::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, + event.x, event.y, intersection)) + { + pos_ = intersection; + arrow_->setPosition(pos_); + arrow_->setOrientation( + Ogre::Quaternion(Ogre::Radian(0.0), Ogre::Vector3::UNIT_Z) * + orient_x); + arrow_->getSceneNode()->setVisible(true); + state_ = Height; + flags |= Render; + } + initz = pos_.z; + prevz = event.y; + } + } + else if (event.type == QEvent::MouseMove && event.right()) + { + if (state_ == Height) + { + double z = event.y; + double dz = z - prevz; + prevz = z; + pos_.z -= dz / z_scale; + arrow_->set(pos_.z, 0.08, 0, 0); + flags |= Render; + } + } + if (event.rightUp()) + { + onPoseSet(pos_.x, pos_.y, pos_.z, prevangle); + + state_ = None; + if (event.shift()) + { + arrow_array.push_back(arrow_); + z_vector.push_back(pos_.z); + arrow_ = new rviz::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f); + arrow_->setColor(0.3f, 0.35f, 0.9f, 1.0f); + arrow_->getSceneNode()->setVisible(false); + } + else + { + sendMessage(); + } + flags |= Render; + } + + if (!event.shift()) + { + if (state_ == None && arrow_array.size() != 0) + { + sendMessage(); + } + } + + sel_manager->highlight(event.viewport, event.x, event.y, event.x, event.y); + } + + return flags; +} + +int +GameLikeInput::processKeyEvent(QKeyEvent* event, rviz::RenderPanel* panel) +{ + return SelectionTool::processKeyEvent(event, panel); +} + +#include +PLUGINLIB_EXPORT_CLASS(GameLikeInput, rviz::Tool) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/gamelikeinput.hpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/gamelikeinput.hpp new file mode 100644 index 0000000..1c8baee --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/gamelikeinput.hpp @@ -0,0 +1,98 @@ +#ifndef GAMELIKEINPUT_HPP +#define GAMELIKEINPUT_HPP + +#ifndef Q_MOC_RUN +#include + +#include "rviz/default_plugin/tools/selection_tool.h" +#include "rviz/properties/float_property.h" +#include "rviz/selection/forwards.h" +#include "rviz/selection/selection_manager.h" +#include "rviz/tool.h" + +#include "rviz/default_plugin/tools/interaction_tool.h" + +#include + +#endif + +namespace rviz +{ +class Arrow; +class StringProperty; +} // namespace rviz + +class GameLikeInput : public rviz::SelectionTool +{ + Q_OBJECT + +protected Q_SLOTS: + void updateTopic(); + +public: + GameLikeInput(); + virtual ~GameLikeInput(); + + virtual void onInitialize(); + + virtual int processMouseEvent(rviz::ViewportMouseEvent& event); + virtual int processKeyEvent(QKeyEvent* event, rviz::RenderPanel* panel); + + void sendMessage(); + +protected: + virtual void onPoseSet(double x, double y, double z_vector, double theta); + +private: + rviz::InteractionTool* move_tool_; + + bool selecting_; + int sel_start_x_; + int sel_start_y_; + + rviz::M_Picked selection_; + + bool moving_; + + ros::NodeHandle nh_; + ros::Publisher pub_pointlist; + ros::Publisher pub_selection; + ros::Publisher pub_swarm; + + double z_max; + double z_min; + double x_max; + double x_min; + double y_max; + double y_min; + + rviz::FloatProperty* property_z_max; + rviz::FloatProperty* property_z_min; + rviz::FloatProperty* property_x_max; + rviz::FloatProperty* property_x_min; + rviz::FloatProperty* property_y_max; + rviz::FloatProperty* property_y_min; + + rviz::StringProperty* topic_property_wp_; + rviz::StringProperty* topic_property_drone_; + rviz::StringProperty* topic_property_swarm_; + +private: + rviz::Arrow* arrow_; + std::vector arrow_array; + std::vector z_vector; + + enum State + { + None, + Position, + Height + }; + State state_; + + Ogre::Vector3 pos_; + + // boost::recursive_mutex global_mutex_; +}; + +#endif // GAMELIKEINPUT_HPP diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/goal_tool.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/goal_tool.cpp new file mode 100755 index 0000000..a567658 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/goal_tool.cpp @@ -0,0 +1,81 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the Willow Garage, 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. + */ + +#include + +#include + +#include "rviz/display_context.h" +#include "rviz/properties/string_property.h" + +#include "goal_tool.h" + +namespace rviz +{ + +Goal3DTool::Goal3DTool() +{ + shortcut_key_ = 'g'; + + topic_property_ = new StringProperty( "Topic", "goal", + "The topic on which to publish navigation goals.", + getPropertyContainer(), SLOT( updateTopic() ), this ); +} + +void Goal3DTool::onInitialize() +{ + Pose3DTool::onInitialize(); + setName( "3D Nav Goal" ); + updateTopic(); +} + +void Goal3DTool::updateTopic() +{ + pub_ = nh_.advertise( topic_property_->getStdString(), 1 ); +} + +void Goal3DTool::onPoseSet(double x, double y, double z, double theta) +{ + ROS_WARN("3D Goal Set"); + std::string fixed_frame = context_->getFixedFrame().toStdString(); + tf::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + tf::Stamped p = tf::Stamped(tf::Pose(quat, tf::Point(x, y, z)), ros::Time::now(), fixed_frame); + geometry_msgs::PoseStamped goal; + tf::poseStampedTFToMsg(p, goal); + ROS_INFO("Setting goal: Frame:%s, Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", fixed_frame.c_str(), + goal.pose.position.x, goal.pose.position.y, goal.pose.position.z, + goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w, theta); + pub_.publish(goal); +} + +} // end namespace rviz + +#include +PLUGINLIB_EXPORT_CLASS( rviz::Goal3DTool, rviz::Tool ) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/goal_tool.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/goal_tool.h new file mode 100755 index 0000000..dd69e07 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/goal_tool.h @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * 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 the Willow Garage, 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. + */ + +#ifndef RVIZ_GOAL_TOOL_H +#define RVIZ_GOAL_TOOL_H + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +# include + +# include + +# include "pose_tool.h" +#endif + +namespace rviz +{ +class Arrow; +class DisplayContext; +class StringProperty; + +class Goal3DTool: public Pose3DTool +{ +Q_OBJECT +public: + Goal3DTool(); + virtual ~Goal3DTool() {} + virtual void onInitialize(); + +protected: + virtual void onPoseSet(double x, double y, double z, double theta); + +private Q_SLOTS: + void updateTopic(); + +private: + ros::NodeHandle nh_; + ros::Publisher pub_; + + StringProperty* topic_property_; +}; + +} + +#endif + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.cpp new file mode 100755 index 0000000..131b118 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.cpp @@ -0,0 +1,365 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the Willow Garage, 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. + */ + +#include + +#include +#include +#include +#include +#include + +#include + +#include + +#include "rviz/frame_manager.h" +#include "rviz/ogre_helpers/grid.h" +#include "rviz/properties/float_property.h" +#include "rviz/properties/int_property.h" +#include "rviz/properties/property.h" +#include "rviz/properties/quaternion_property.h" +#include "rviz/properties/ros_topic_property.h" +#include "rviz/properties/vector_property.h" +#include "rviz/validate_floats.h" +#include "rviz/display_context.h" + +#include "multi_probmap_display.h" + +namespace rviz +{ + +MultiProbMapDisplay::MultiProbMapDisplay() + : Display() + , loaded_( false ) + , new_map_(false) +{ + topic_property_ = new RosTopicProperty( "Topic", "", + QString::fromStdString( ros::message_traits::datatype() ), + "multi_map_server::MultiOccupancyGrid topic to subscribe to.", + this, SLOT( updateTopic() )); + + draw_under_property_ = new Property( "Draw Behind", false, + "Rendering option, controls whether or not the map is always" + " drawn behind everything else.", + this, SLOT( updateDrawUnder() )); +} + +MultiProbMapDisplay::~MultiProbMapDisplay() +{ + unsubscribe(); + clear(); +} + +void MultiProbMapDisplay::onInitialize() +{ +} + +void MultiProbMapDisplay::onEnable() +{ + subscribe(); +} + +void MultiProbMapDisplay::onDisable() +{ + unsubscribe(); + clear(); +} + +void MultiProbMapDisplay::subscribe() +{ + if ( !isEnabled() ) + { + return; + } + + if( !topic_property_->getTopic().isEmpty() ) + { + try + { + map_sub_ = update_nh_.subscribe( topic_property_->getTopicStd(), 1, &MultiProbMapDisplay::incomingMap, this ); + setStatus( StatusProperty::Ok, "Topic", "OK" ); + } + catch( ros::Exception& e ) + { + setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() ); + } + } +} + +void MultiProbMapDisplay::unsubscribe() +{ + map_sub_.shutdown(); +} + +void MultiProbMapDisplay::updateDrawUnder() +{ + bool draw_under = draw_under_property_->getValue().toBool(); + + for (unsigned int k = 0; k < material_.size(); k++) + material_[k]->setDepthWriteEnabled( !draw_under ); + + for (unsigned int k = 0; k < manual_object_.size(); k++) + { + if( draw_under ) + manual_object_[k]->setRenderQueueGroup( Ogre::RENDER_QUEUE_4 ); + else + manual_object_[k]->setRenderQueueGroup( Ogre::RENDER_QUEUE_MAIN ); + } +} + +void MultiProbMapDisplay::updateTopic() +{ + unsubscribe(); + subscribe(); + clear(); +} + +void MultiProbMapDisplay::clear() +{ + setStatus( StatusProperty::Warn, "Message", "No map received" ); + + if( !loaded_ ) + { + return; + } + + for (unsigned k = 0; k < manual_object_.size(); k++) + { + scene_manager_->destroyManualObject( manual_object_[k] ); + std::string tex_name = texture_[k]->getName(); + texture_[k].setNull(); + Ogre::TextureManager::getSingleton().unload( tex_name ); + } + manual_object_.clear(); + texture_.clear(); + material_.clear(); + + loaded_ = false; +} + +// *********************************************************************************************************************************** + +void MultiProbMapDisplay::update( float wall_dt, float ros_dt ) +{ + { + boost::mutex::scoped_lock lock(mutex_); + current_map_ = updated_map_; + } + + if (!new_map_) + return; + new_map_ = false; + + clear(); + + //ros::Time t[5]; + //double dt[4] = {0,0,0,0}; + for (unsigned int k = 0; k < current_map_->maps.size(); k++) + { + if (current_map_->maps[k].data.empty()) + continue; + setStatus( StatusProperty::Ok, "Message", "Map received" ); + + // Map info + float resolution = current_map_->maps[k].info.resolution; + int width = current_map_->maps[k].info.width; + int height = current_map_->maps[k].info.height; + + // Load pixel + //t[0] = ros::Time::now(); + unsigned int pixels_size = width * height; + unsigned char* pixels = new unsigned char[pixels_size]; + memset(pixels, 255, pixels_size); + unsigned int num_pixels_to_copy = pixels_size; + if( pixels_size != current_map_->maps[k].data.size() ) + if( current_map_->maps[k].data.size() < pixels_size ) + num_pixels_to_copy = current_map_->maps[k].data.size(); + for( unsigned int pixel_index = 0; pixel_index < num_pixels_to_copy; pixel_index++ ) + { + unsigned char val; + int8_t data = current_map_->maps[k].data[ pixel_index ]; + if(data > 0) + val = 255; + else if(data < 0) + val = 180; + else + val = 0; + pixels[ pixel_index ] = val; + } +/* + int pixels_size = current_map_->maps[k].data.size(); + unsigned char* pixels = new unsigned char[pixels_size]; + memcpy(pixels, ¤t_map_->maps[k].data[0], pixels_size); +*/ + // Set texture + //t[1] = ros::Time::now(); + Ogre::DataStreamPtr pixel_stream; + pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size )); + static int tex_count = 0; + std::stringstream ss1; + ss1 << "MultiMapTexture" << tex_count++; + Ogre::TexturePtr _texture_; + //t[2] = ros::Time::now(); + _texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss1.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D, 0); + //t[3] = ros::Time::now(); + texture_.push_back(_texture_); + delete [] pixels; + setStatus( StatusProperty::Ok, "Map", "Map OK" ); + //t[4] = ros::Time::now(); + + // Set material + static int material_count = 0; + std::stringstream ss0; + ss0 << "MultiMapObjectMaterial" << material_count++; + Ogre::MaterialPtr _material_; + _material_ = Ogre::MaterialManager::getSingleton().create( ss0.str(), + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME ); + _material_->setReceiveShadows(false); + _material_->getTechnique(0)->setLightingEnabled(false); + _material_->setDepthBias( -16.0f, 0.0f ); + _material_->setCullingMode( Ogre::CULL_NONE ); + _material_->setDepthWriteEnabled(false); + material_.push_back(_material_); + material_.back()->setSceneBlending( Ogre::SBT_TRANSPARENT_COLOUR ); + material_.back()->setDepthWriteEnabled( false ); + Ogre::Pass* pass = material_.back()->getTechnique(0)->getPass(0); + Ogre::TextureUnitState* tex_unit = NULL; + if (pass->getNumTextureUnitStates() > 0) + tex_unit = pass->getTextureUnitState(0); + else + tex_unit = pass->createTextureUnitState(); + tex_unit->setTextureName(texture_.back()->getName()); + tex_unit->setTextureFiltering( Ogre::TFO_NONE ); + + // Set manual object + static int map_count = 0; + std::stringstream ss2; + ss2 << "MultiMapObject" << map_count++; + Ogre::ManualObject* _manual_object_ = scene_manager_->createManualObject( ss2.str() ); + manual_object_.push_back(_manual_object_); + scene_node_->attachObject( manual_object_.back() ); + float yo = tf::getYaw(current_map_->origins[k].orientation); + float co = cos(yo); + float so = sin(yo); + float dxo = current_map_->origins[k].position.x; + float dyo = current_map_->origins[k].position.y; + float ym = tf::getYaw(current_map_->maps[k].info.origin.orientation); + float dxm = current_map_->maps[k].info.origin.position.x; + float dym = current_map_->maps[k].info.origin.position.y; + float yaw = yo + ym; + float c = cos(yaw); + float s = sin(yaw); + float dx = co * dxm - so * dym + dxo; + float dy = so * dxm + co * dym + dyo; + float x = 0.0; + float y = 0.0; + manual_object_.back()->begin(material_.back()->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); + { + // First triangle + { + // Bottom left + x = c * 0.0 - s * 0.0 + dx; + y = s * 0.0 + c * 0.0 + dy; + manual_object_.back()->position( x, y, 0.0f ); + manual_object_.back()->textureCoord(0.0f, 0.0f); + manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); + + // Top right + x = c * resolution*width - s * resolution*height + dx; + y = s * resolution*width + c * resolution*height + dy; + manual_object_.back()->position( x, y, 0.0f ); + manual_object_.back()->textureCoord(1.0f, 1.0f); + manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); + + // Top left + x = c * 0.0 - s * resolution*height + dx; + y = s * 0.0 + c * resolution*height + dy; + manual_object_.back()->position( x, y, 0.0f ); + manual_object_.back()->textureCoord(0.0f, 1.0f); + manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); + } + + // Second triangle + { + // Bottom left + x = c * 0.0 - s * 0.0 + dx; + y = s * 0.0 + c * 0.0 + dy; + manual_object_.back()->position( x, y, 0.0f ); + manual_object_.back()->textureCoord(0.0f, 0.0f); + manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); + + // Bottom right + x = c * resolution*width - s * 0.0 + dx; + y = s * resolution*width + c * 0.0 + dy; + manual_object_.back()->position( x, y, 0.0f ); + manual_object_.back()->textureCoord(1.0f, 0.0f); + manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); + + // Top right + x = c * resolution*width - s * resolution*height + dx; + y = s * resolution*width + c * resolution*height + dy; + manual_object_.back()->position( x, y, 0.0f ); + manual_object_.back()->textureCoord(1.0f, 1.0f); + manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); + } + } + manual_object_.back()->end(); + if( draw_under_property_->getValue().toBool() ) + manual_object_.back()->setRenderQueueGroup(Ogre::RENDER_QUEUE_4); + + //for (int i = 0; i < 4; i++) + // dt[i] += (t[i+1] - t[i]).toSec(); + } + loaded_ = true; + context_->queueRender(); + //ROS_ERROR("RVIZ MAP: %f %f %f %f", dt[0],dt[1],dt[2],dt[3]); +} + +// *********************************************************************************************************************************** + +void MultiProbMapDisplay::incomingMap(const multi_map_server::MultiOccupancyGrid::ConstPtr& msg) +{ + updated_map_ = msg; + boost::mutex::scoped_lock lock(mutex_); + new_map_ = true; +} + +void MultiProbMapDisplay::reset() +{ + Display::reset(); + clear(); + updateTopic(); +} + +} // namespace rviz + +#include +PLUGINLIB_EXPORT_CLASS( rviz::MultiProbMapDisplay, rviz::Display ) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.h new file mode 100755 index 0000000..7c23b29 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/multi_probmap_display.h @@ -0,0 +1,113 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * 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 the Willow Garage, 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. + */ + +#ifndef MULTI_PROB_MAP_DISPLAY_H +#define MULTI_PROB_MAP_DISPLAY_H + +#include +#include +#include + +#include +#include + +#include +#include + +#include "rviz/display.h" + +namespace Ogre +{ +class ManualObject; +} + +namespace rviz +{ + +class FloatProperty; +class IntProperty; +class Property; +class QuaternionProperty; +class RosTopicProperty; +class VectorProperty; + +/** + * \class MultiProbMapDisplay + * \brief Displays a map along the XY plane. + */ +class MultiProbMapDisplay : public Display +{ + Q_OBJECT +public: + MultiProbMapDisplay(); + virtual ~MultiProbMapDisplay(); + + // Overrides from Display + virtual void onInitialize(); + virtual void reset(); + virtual void update(float wall_dt, float ros_dt); + +protected Q_SLOTS: + void updateTopic(); + void updateDrawUnder(); + +protected: + // overrides from Display + virtual void onEnable(); + virtual void onDisable(); + + virtual void subscribe(); + virtual void unsubscribe(); + + void incomingMap(const multi_map_server::MultiOccupancyGrid::ConstPtr& msg); + + void clear(); + + std::vector manual_object_; + std::vector texture_; + std::vector material_; + + bool loaded_; + + std::string topic_; + + ros::Subscriber map_sub_; + + RosTopicProperty* topic_property_; + Property* draw_under_property_; + + multi_map_server::MultiOccupancyGrid::ConstPtr updated_map_; + multi_map_server::MultiOccupancyGrid::ConstPtr current_map_; + boost::mutex mutex_; + bool new_map_; +}; + +} // namespace rviz + +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/pose_tool.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/pose_tool.cpp new file mode 100755 index 0000000..e7dd561 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/pose_tool.cpp @@ -0,0 +1,170 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the Willow Garage, 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. + */ + +#include +#include +#include +#include + +#include "rviz/geometry.h" +#include "rviz/load_resource.h" +#include "rviz/ogre_helpers/arrow.h" +#include "rviz/render_panel.h" +#include "rviz/viewport_mouse_event.h" + +#include "pose_tool.h" + +namespace rviz +{ + +Pose3DTool::Pose3DTool() + : Tool() + , arrow_(NULL) +{ +} + +Pose3DTool::~Pose3DTool() +{ + delete arrow_; +} + +void +Pose3DTool::onInitialize() +{ + arrow_ = new Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f); + arrow_->setColor(0.0f, 1.0f, 0.0f, 1.0f); + arrow_->getSceneNode()->setVisible(false); +} + +void +Pose3DTool::activate() +{ + setStatus("Click and drag mouse to set position/orientation."); + state_ = Position; +} + +void +Pose3DTool::deactivate() +{ + arrow_->getSceneNode()->setVisible(false); +} + +int +Pose3DTool::processMouseEvent(ViewportMouseEvent& event) +{ + int flags = 0; + static Ogre::Vector3 ang_pos; + static double initz; + static double prevz; + static double prevangle; + const double z_scale = 50; + const double z_interval = 0.5; + Ogre::Quaternion orient_x = + Ogre::Quaternion(Ogre::Radian(Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Z); + + if (event.leftDown()) + { + ROS_ASSERT(state_ == Position); + Ogre::Vector3 intersection; + Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); + if (getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, + event.y, intersection)) + { + pos_ = intersection; + arrow_->setPosition(pos_); + state_ = Orientation; + flags |= Render; + } + } + else if (event.type == QEvent::MouseMove && event.left()) + { + if (state_ == Orientation) + { + // compute angle in x-y plane + Ogre::Vector3 cur_pos; + Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); + if (getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, + event.y, cur_pos)) + { + double angle = atan2(cur_pos.y - pos_.y, cur_pos.x - pos_.x); + arrow_->getSceneNode()->setVisible(true); + arrow_->setOrientation(Ogre::Quaternion(orient_x)); + if (event.right()) + state_ = Height; + initz = pos_.z; + prevz = event.y; + prevangle = angle; + flags |= Render; + } + } + if (state_ == Height) + { + double z = event.y; + double dz = z - prevz; + prevz = z; + pos_.z -= dz / z_scale; + arrow_->setPosition(pos_); + // Create a list of arrows + for (int k = 0; k < arrow_array.size(); k++) + delete arrow_array[k]; + arrow_array.clear(); + int cnt = ceil(fabs(initz - pos_.z) / z_interval); + for (int k = 0; k < cnt; k++) + { + Arrow* arrow__; + arrow__ = new Arrow(scene_manager_, NULL, 0.5f, 0.1f, 0.0f, 0.1f); + arrow__->setColor(0.0f, 1.0f, 0.0f, 1.0f); + arrow__->getSceneNode()->setVisible(true); + Ogre::Vector3 arr_pos = pos_; + arr_pos.z = initz - ((initz - pos_.z > 0) ? 1 : -1) * k * z_interval; + arrow__->setPosition(arr_pos); + arrow__->setOrientation( + Ogre::Quaternion(Ogre::Radian(prevangle), Ogre::Vector3::UNIT_Z) * + orient_x); + arrow_array.push_back(arrow__); + } + flags |= Render; + } + } + else if (event.leftUp()) + { + if (state_ == Orientation || state_ == Height) + { + // Create a list of arrows + for (int k = 0; k < arrow_array.size(); k++) + delete arrow_array[k]; + arrow_array.clear(); + onPoseSet(pos_.x, pos_.y, pos_.z, prevangle); + flags |= (Finished | Render); + } + } + + return flags; +} +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/pose_tool.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/pose_tool.h new file mode 100755 index 0000000..ff9f08d --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/pose_tool.h @@ -0,0 +1,77 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the Willow Garage, 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. + */ + +#ifndef RVIZ_POSE_TOOL_H +#define RVIZ_POSE_TOOL_H + +#include + +#include + +#include + +#include "rviz/tool.h" + +namespace rviz +{ +class Arrow; +class DisplayContext; + +class Pose3DTool : public Tool +{ +public: + Pose3DTool(); + virtual ~Pose3DTool(); + + virtual void onInitialize(); + + virtual void activate(); + virtual void deactivate(); + + virtual int processMouseEvent(ViewportMouseEvent& event); + +protected: + virtual void onPoseSet(double x, double y, double z, double theta) = 0; + + Arrow* arrow_; + std::vector arrow_array; + + enum State + { + Position, + Orientation, + Height + }; + State state_; + + Ogre::Vector3 pos_; +}; +} + +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/probmap_display.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/probmap_display.cpp new file mode 100755 index 0000000..77f41db --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/probmap_display.cpp @@ -0,0 +1,566 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * 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 the Willow Garage, 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. + */ + +#include + +#include +#include +#include +#include +#include + +#include + +#include + +#include "rviz/display_context.h" +#include "rviz/frame_manager.h" +#include "rviz/ogre_helpers/grid.h" +#include "rviz/properties/float_property.h" +#include "rviz/properties/int_property.h" +#include "rviz/properties/property.h" +#include "rviz/properties/quaternion_property.h" +#include "rviz/properties/ros_topic_property.h" +#include "rviz/properties/vector_property.h" +#include "rviz/validate_floats.h" + +#include "probmap_display.h" + +namespace rviz +{ + +ProbMapDisplay::ProbMapDisplay() + : Display() + , manual_object_(NULL) + //! @bug cannot compile @gcc-5 or later, material_(0) + , loaded_(false) + , resolution_(0.0f) + , width_(0) + , height_(0) + , position_(Ogre::Vector3::ZERO) + , orientation_(Ogre::Quaternion::IDENTITY) + , new_map_(false) +{ + topic_property_ = new RosTopicProperty( + "Topic", "", QString::fromStdString( + ros::message_traits::datatype()), + "nav_msgs::OccupancyGrid topic to subscribe to.", this, + SLOT(updateTopic())); + + alpha_property_ = new FloatProperty( + "Alpha", 0.7, "Amount of transparency to apply to the map.", this, + SLOT(updateAlpha())); + alpha_property_->setMin(0); + alpha_property_->setMax(1); + + draw_under_property_ = + new Property("Draw Behind", false, + "Rendering option, controls whether or not the map is always" + " drawn behind everything else.", + this, SLOT(updateDrawUnder())); + + resolution_property_ = new FloatProperty( + "Resolution", 0, "Resolution of the map. (not editable)", this); + resolution_property_->setReadOnly(true); + + width_property_ = new IntProperty( + "Width", 0, "Width of the map, in meters. (not editable)", this); + width_property_->setReadOnly(true); + + height_property_ = new IntProperty( + "Height", 0, "Height of the map, in meters. (not editable)", this); + height_property_->setReadOnly(true); + + position_property_ = new VectorProperty( + "Position", Ogre::Vector3::ZERO, + "Position of the bottom left corner of the map, in meters. (not editable)", + this); + position_property_->setReadOnly(true); + + orientation_property_ = + new QuaternionProperty("Orientation", Ogre::Quaternion::IDENTITY, + "Orientation of the map. (not editable)", this); + orientation_property_->setReadOnly(true); +} + +ProbMapDisplay::~ProbMapDisplay() +{ + unsubscribe(); + clear(); +} + +void +ProbMapDisplay::onInitialize() +{ + static int count = 0; + std::stringstream ss; + ss << "MapObjectMaterial" << count++; + material_ = Ogre::MaterialManager::getSingleton().create( + ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material_->setReceiveShadows(false); + material_->getTechnique(0)->setLightingEnabled(false); + material_->setDepthBias(-16.0f, 0.0f); + material_->setCullingMode(Ogre::CULL_NONE); + material_->setDepthWriteEnabled(false); + + updateAlpha(); +} + +void +ProbMapDisplay::onEnable() +{ + subscribe(); +} + +void +ProbMapDisplay::onDisable() +{ + unsubscribe(); + clear(); +} + +void +ProbMapDisplay::subscribe() +{ + if (!isEnabled()) + { + return; + } + + if (!topic_property_->getTopic().isEmpty()) + { + try + { + map_sub_ = update_nh_.subscribe(topic_property_->getTopicStd(), 1, + &ProbMapDisplay::incomingMap, this); + setStatus(StatusProperty::Ok, "Topic", "OK"); + } + catch (ros::Exception& e) + { + setStatus(StatusProperty::Error, "Topic", + QString("Error subscribing: ") + e.what()); + } + } +} + +void +ProbMapDisplay::unsubscribe() +{ + map_sub_.shutdown(); +} + +void +ProbMapDisplay::updateAlpha() +{ + float alpha = alpha_property_->getFloat(); + + Ogre::Pass* pass = material_->getTechnique(0)->getPass(0); + Ogre::TextureUnitState* tex_unit = NULL; + if (pass->getNumTextureUnitStates() > 0) + { + tex_unit = pass->getTextureUnitState(0); + } + else + { + tex_unit = pass->createTextureUnitState(); + } + + tex_unit->setAlphaOperation(Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, + Ogre::LBS_CURRENT, alpha); + + if (alpha < 0.9998) + { + material_->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material_->setDepthWriteEnabled(false); + } + else + { + material_->setSceneBlending(Ogre::SBT_REPLACE); + material_->setDepthWriteEnabled(!draw_under_property_->getValue().toBool()); + } +} + +void +ProbMapDisplay::updateDrawUnder() +{ + bool draw_under = draw_under_property_->getValue().toBool(); + + if (alpha_property_->getFloat() >= 0.9998) + { + material_->setDepthWriteEnabled(!draw_under); + } + + if (manual_object_) + { + if (draw_under) + { + manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4); + } + else + { + manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_MAIN); + } + } +} + +void +ProbMapDisplay::updateTopic() +{ + unsubscribe(); + subscribe(); + clear(); +} + +void +ProbMapDisplay::clear() +{ + setStatus(StatusProperty::Warn, "Message", "No map received"); + + if (!loaded_) + { + return; + } + + scene_manager_->destroyManualObject(manual_object_); + manual_object_ = NULL; + + std::string tex_name = texture_->getName(); + texture_.setNull(); + Ogre::TextureManager::getSingleton().unload(tex_name); + + loaded_ = false; +} + +bool +validateFloats(const nav_msgs::OccupancyGrid& msg) +{ + bool valid = true; + valid = valid && validateFloats(msg.info.resolution); + valid = valid && validateFloats(msg.info.origin); + return valid; +} + +void +ProbMapDisplay::update(float wall_dt, float ros_dt) +{ + { + boost::mutex::scoped_lock lock(mutex_); + + current_map_ = updated_map_; + } + + if (!current_map_ || !new_map_) + { + return; + } + + if (current_map_->data.empty()) + { + return; + } + + new_map_ = false; + + if (!validateFloats(*current_map_)) + { + setStatus(StatusProperty::Error, "Map", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + if (current_map_->info.width * current_map_->info.height == 0) + { + std::stringstream ss; + ss << "Map is zero-sized (" << current_map_->info.width << "x" + << current_map_->info.height << ")"; + setStatus(StatusProperty::Error, "Map", QString::fromStdString(ss.str())); + return; + } + + clear(); + + setStatus(StatusProperty::Ok, "Message", "Map received"); + + ROS_DEBUG("Received a %d X %d map @ %.3f m/pix\n", current_map_->info.width, + current_map_->info.height, current_map_->info.resolution); + + float resolution = current_map_->info.resolution; + + int width = current_map_->info.width; + int height = current_map_->info.height; + + Ogre::Vector3 position(current_map_->info.origin.position.x, + current_map_->info.origin.position.y, + current_map_->info.origin.position.z); + Ogre::Quaternion orientation(current_map_->info.origin.orientation.w, + current_map_->info.origin.orientation.x, + current_map_->info.origin.orientation.y, + current_map_->info.origin.orientation.z); + frame_ = current_map_->header.frame_id; + if (frame_.empty()) + { + frame_ = "/map"; + } + + // Expand it to be RGB data + unsigned int pixels_size = width * height; + unsigned char* pixels = new unsigned char[pixels_size]; + memset(pixels, 255, pixels_size); + + bool map_status_set = false; + unsigned int num_pixels_to_copy = pixels_size; + if (pixels_size != current_map_->data.size()) + { + std::stringstream ss; + ss << "Data size doesn't match width*height: width = " << width + << ", height = " << height + << ", data size = " << current_map_->data.size(); + setStatus(StatusProperty::Error, "Map", QString::fromStdString(ss.str())); + map_status_set = true; + + // Keep going, but don't read past the end of the data. + if (current_map_->data.size() < pixels_size) + { + num_pixels_to_copy = current_map_->data.size(); + } + } + + // TODO: a fragment shader could do this on the video card, and + // would allow a non-grayscale color to mark the out-of-range + // values. + for (unsigned int pixel_index = 0; pixel_index < num_pixels_to_copy; + pixel_index++) + { + unsigned char val; + int8_t data = current_map_->data[pixel_index]; + if (data > 0) + val = 0; + else if (data < 0) + val = 255; + else + val = 127; + pixels[pixel_index] = val; + } + + Ogre::DataStreamPtr pixel_stream; + pixel_stream.bind(new Ogre::MemoryDataStream(pixels, pixels_size)); + static int tex_count = 0; + std::stringstream ss; + ss << "MapTexture" << tex_count++; + try + { + texture_ = Ogre::TextureManager::getSingleton().loadRawData( + ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D, 0); + + if (!map_status_set) + { + setStatus(StatusProperty::Ok, "Map", "Map OK"); + } + } + catch (Ogre::RenderingAPIException&) + { + Ogre::Image image; + pixel_stream->seek(0); + float fwidth = width; + float fheight = height; + if (width > height) + { + float aspect = fheight / fwidth; + fwidth = 2048; + fheight = fwidth * aspect; + } + else + { + float aspect = fwidth / fheight; + fheight = 2048; + fwidth = fheight * aspect; + } + + { + std::stringstream ss; + ss + << "Map is larger than your graphics card supports. Downsampled from [" + << width << "x" << height << "] to [" << fwidth << "x" << fheight + << "]"; + setStatus(StatusProperty::Ok, "Map", QString::fromStdString(ss.str())); + } + + ROS_WARN("Failed to create full-size map texture, likely because your " + "graphics card does not support textures of size > 2048. " + "Downsampling to [%d x %d]...", + (int)fwidth, (int)fheight); + // ROS_INFO("Stream size [%d], width [%f], height [%f], w * h [%f]", + // pixel_stream->size(), width, height, width * height); + image.loadRawData(pixel_stream, width, height, Ogre::PF_L8); + image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST); + ss << "Downsampled"; + texture_ = Ogre::TextureManager::getSingleton().loadImage( + ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image); + } + + delete[] pixels; + + Ogre::Pass* pass = material_->getTechnique(0)->getPass(0); + Ogre::TextureUnitState* tex_unit = NULL; + if (pass->getNumTextureUnitStates() > 0) + { + tex_unit = pass->getTextureUnitState(0); + } + else + { + tex_unit = pass->createTextureUnitState(); + } + + tex_unit->setTextureName(texture_->getName()); + tex_unit->setTextureFiltering(Ogre::TFO_NONE); + + static int map_count = 0; + std::stringstream ss2; + ss2 << "MapObject" << map_count++; + manual_object_ = scene_manager_->createManualObject(ss2.str()); + scene_node_->attachObject(manual_object_); + + manual_object_->begin(material_->getName(), + Ogre::RenderOperation::OT_TRIANGLE_LIST); + { + // First triangle + { + // Bottom left + manual_object_->position(0.0f, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 0.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Top right + manual_object_->position(resolution * width, resolution * height, 0.0f); + manual_object_->textureCoord(1.0f, 1.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Top left + manual_object_->position(0.0f, resolution * height, 0.0f); + manual_object_->textureCoord(0.0f, 1.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + } + + // Second triangle + { + // Bottom left + manual_object_->position(0.0f, 0.0f, 0.0f); + manual_object_->textureCoord(0.0f, 0.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Bottom right + manual_object_->position(resolution * width, 0.0f, 0.0f); + manual_object_->textureCoord(1.0f, 0.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + + // Top right + manual_object_->position(resolution * width, resolution * height, 0.0f); + manual_object_->textureCoord(1.0f, 1.0f); + manual_object_->normal(0.0f, 0.0f, 1.0f); + } + } + manual_object_->end(); + + if (draw_under_property_->getValue().toBool()) + { + manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4); + } + + resolution_property_->setValue(resolution); + width_property_->setValue(width); + height_property_->setValue(height); + position_property_->setVector(position); + orientation_property_->setQuaternion(orientation); + + transformMap(); + + loaded_ = true; + + context_->queueRender(); +} + +void +ProbMapDisplay::incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg) +{ + + updated_map_ = msg; + boost::mutex::scoped_lock lock(mutex_); + new_map_ = true; +} + +void +ProbMapDisplay::transformMap() +{ + if (!current_map_) + { + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->transform( + frame_, ros::Time(), current_map_->info.origin, position, orientation)) + { + ROS_DEBUG("Error transforming map '%s' from frame '%s' to frame '%s'", + qPrintable(getName()), frame_.c_str(), qPrintable(fixed_frame_)); + + setStatus(StatusProperty::Error, "Transform", + "No transform from [" + QString::fromStdString(frame_) + + "] to [" + fixed_frame_ + "]"); + } + else + { + setStatus(StatusProperty::Ok, "Transform", "Transform OK"); + } + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); +} + +void +ProbMapDisplay::fixedFrameChanged() +{ + transformMap(); +} + +void +ProbMapDisplay::reset() +{ + Display::reset(); + + clear(); + // Force resubscription so that the map will be re-sent + updateTopic(); +} + +} // namespace rviz + +#include +PLUGINLIB_EXPORT_CLASS(rviz::ProbMapDisplay, rviz::Display) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/probmap_display.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/probmap_display.h new file mode 100755 index 0000000..332def2 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/rviz_plugins/src/probmap_display.h @@ -0,0 +1,134 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * 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 the Willow Garage, 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. + */ + +#ifndef PROB_MAP_DISPLAY_H +#define PROB_MAP_DISPLAY_H + +#include +#include +#include + +#include +#include + +#include + +#include "rviz/display.h" + +namespace Ogre +{ +class ManualObject; +} + +namespace rviz +{ + +class FloatProperty; +class IntProperty; +class Property; +class QuaternionProperty; +class RosTopicProperty; +class VectorProperty; + +/** + * \class ProbMapDisplay + * \brief Displays a map along the XY plane. + */ +class ProbMapDisplay: public Display +{ +Q_OBJECT +public: + ProbMapDisplay(); + virtual ~ProbMapDisplay(); + + // Overrides from Display + virtual void onInitialize(); + virtual void fixedFrameChanged(); + virtual void reset(); + virtual void update( float wall_dt, float ros_dt ); + + float getResolution() { return resolution_; } + int getWidth() { return width_; } + int getHeight() { return height_; } + Ogre::Vector3 getPosition() { return position_; } + Ogre::Quaternion getOrientation() { return orientation_; } + +protected Q_SLOTS: + void updateAlpha(); + void updateTopic(); + void updateDrawUnder(); + + +protected: + // overrides from Display + virtual void onEnable(); + virtual void onDisable(); + + virtual void subscribe(); + virtual void unsubscribe(); + + void incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg); + + void clear(); + + void transformMap(); + + Ogre::ManualObject* manual_object_; + Ogre::TexturePtr texture_; + Ogre::MaterialPtr material_; + bool loaded_; + + std::string topic_; + float resolution_; + int width_; + int height_; + Ogre::Vector3 position_; + Ogre::Quaternion orientation_; + std::string frame_; + + ros::Subscriber map_sub_; + + RosTopicProperty* topic_property_; + FloatProperty* resolution_property_; + IntProperty* width_property_; + IntProperty* height_property_; + VectorProperty* position_property_; + QuaternionProperty* orientation_property_; + FloatProperty* alpha_property_; + Property* draw_under_property_; + + nav_msgs::OccupancyGrid::ConstPtr updated_map_; + nav_msgs::OccupancyGrid::ConstPtr current_map_; + boost::mutex mutex_; + bool new_map_; +}; + +} // namespace rviz + + #endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/CMakeLists.txt new file mode 100644 index 0000000..6a90552 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/CMakeLists.txt @@ -0,0 +1,100 @@ +cmake_minimum_required(VERSION 2.8.3) +project(uav_utils) + +## Turn on verbose output +set(CMAKE_VERBOSE_MAKEFILE "false") + +## Enable C++11 +include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) +if(COMPILER_SUPPORTS_CXX11) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +elseif(COMPILER_SUPPORTS_CXX0X) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +else() + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") +endif() + +## Add some compile flags +set(ADDITIONAL_CXX_FLAG "-Wall -O3 -g") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CXX_FLAG}") + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + cmake_utils +) + + +find_package(Eigen REQUIRED) # try to find manually installed eigen (Usually in /usr/local with provided FindEigen3.cmake) + +################################### +## catkin specific configuration ## +################################### +catkin_package( + INCLUDE_DIRS include + # LIBRARIES uav_utils + CATKIN_DEPENDS roscpp rospy + # DEPENDS system_lib +) + +########### +## Build ## +########### + + +include_directories( + include + ${EIGEN_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +############# +## 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 geometry_utils geometry_utils_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 src/${PROJECT_NAME}_test.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) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/include/uav_utils/converters.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/include/uav_utils/converters.h new file mode 100644 index 0000000..9932731 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/include/uav_utils/converters.h @@ -0,0 +1,95 @@ +#ifndef __UAVUTILS_CONVERTERS_H +#define __UAVUTILS_CONVERTERS_H + +#include +#include +#include +#include + +#include +#include +#include + +namespace uav_utils { + +inline void extract_odometry(nav_msgs::OdometryConstPtr msg, Eigen::Vector3d& p, + Eigen::Vector3d& v, Eigen::Quaterniond& q) +{ + p(0) = msg->pose.pose.position.x; + p(1) = msg->pose.pose.position.y; + p(2) = msg->pose.pose.position.z; + + v(0) = msg->twist.twist.linear.x; + v(1) = msg->twist.twist.linear.y; + v(2) = msg->twist.twist.linear.z; + + q.w() = msg->pose.pose.orientation.w; + q.x() = msg->pose.pose.orientation.x; + q.y() = msg->pose.pose.orientation.y; + q.z() = msg->pose.pose.orientation.z; +} + +inline void extract_odometry(nav_msgs::OdometryConstPtr msg, Eigen::Vector3d& p, + Eigen::Vector3d& v, Eigen::Quaterniond& q, Eigen::Vector3d& w) +{ + extract_odometry(msg, p, v, q); + + w(0) = msg->twist.twist.angular.x; + w(1) = msg->twist.twist.angular.y; + w(2) = msg->twist.twist.angular.z; +} + + +template +Eigen::Matrix from_vector3_msg(const geometry_msgs::Vector3& msg) { + return Eigen::Matrix(msg.x, msg.y, msg.z); +} + +template +geometry_msgs::Vector3 to_vector3_msg(const Eigen::DenseBase& v) { + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); + EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + + geometry_msgs::Vector3 msg; + msg.x = v.x(); + msg.y = v.y(); + msg.z = v.z(); + return msg; +} + +template +Eigen::Matrix from_point_msg(const geometry_msgs::Point& msg) { + return Eigen::Matrix(msg.x, msg.y, msg.z); +} + +template +geometry_msgs::Point to_point_msg(const Eigen::DenseBase& v) { + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); + EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + + geometry_msgs::Point msg; + msg.x = v.x(); + msg.y = v.y(); + msg.z = v.z(); + return msg; +} + +template +Eigen::Quaternion from_quaternion_msg(const geometry_msgs::Quaternion& msg) { + return Eigen::Quaternion(msg.w, msg.x, msg.y, msg.z); +} + +template +geometry_msgs::Quaternion to_quaternion_msg(const Eigen::Quaternion& q) { + geometry_msgs::Quaternion msg; + msg.x = q.x(); + msg.y = q.y(); + msg.z = q.z(); + msg.w = q.w(); + return msg; +} +} + +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/include/uav_utils/geometry_utils.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/include/uav_utils/geometry_utils.h new file mode 100644 index 0000000..c3babb1 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/include/uav_utils/geometry_utils.h @@ -0,0 +1,248 @@ +#ifndef __GEOMETRY_UTILS_H +#define __GEOMETRY_UTILS_H + +#include + +/* clang-format off */ +namespace uav_utils { + +template +Scalar_t toRad(const Scalar_t& x) { + return x / 180.0 * M_PI; +} + +template +Scalar_t toDeg(const Scalar_t& x) { + return x * 180.0 / M_PI; +} + +template +Eigen::Matrix rotx(Scalar_t t) { + Eigen::Matrix R; + R(0, 0) = 1.0; + R(0, 1) = 0.0; + R(0, 2) = 0.0; + R(1, 0) = 0.0; + R(1, 1) = std::cos(t); + R(1, 2) = -std::sin(t); + R(2, 0) = 0.0; + R(2, 1) = std::sin(t); + R(2, 2) = std::cos(t); + + return R; +} + +template +Eigen::Matrix roty(Scalar_t t) { + Eigen::Matrix R; + R(0, 0) = std::cos(t); + R(0, 1) = 0.0; + R(0, 2) = std::sin(t); + R(1, 0) = 0.0; + R(1, 1) = 1.0; + R(1, 2) = 0; + R(2, 0) = -std::sin(t); + R(2, 1) = 0.0; + R(2, 2) = std::cos(t); + + return R; +} + +template +Eigen::Matrix rotz(Scalar_t t) { + Eigen::Matrix R; + R(0, 0) = std::cos(t); + R(0, 1) = -std::sin(t); + R(0, 2) = 0.0; + R(1, 0) = std::sin(t); + R(1, 1) = std::cos(t); + R(1, 2) = 0.0; + R(2, 0) = 0.0; + R(2, 1) = 0.0; + R(2, 2) = 1.0; + + return R; +} + +template +Eigen::Matrix ypr_to_R(const Eigen::DenseBase& ypr) { + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); + EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + + typename Derived::Scalar c, s; + + Eigen::Matrix Rz = Eigen::Matrix::Zero(); + typename Derived::Scalar y = ypr(0); + c = cos(y); + s = sin(y); + Rz(0, 0) = c; + Rz(1, 0) = s; + Rz(0, 1) = -s; + Rz(1, 1) = c; + Rz(2, 2) = 1; + + Eigen::Matrix Ry = Eigen::Matrix::Zero(); + typename Derived::Scalar p = ypr(1); + c = cos(p); + s = sin(p); + Ry(0, 0) = c; + Ry(2, 0) = -s; + Ry(0, 2) = s; + Ry(2, 2) = c; + Ry(1, 1) = 1; + + Eigen::Matrix Rx = Eigen::Matrix::Zero(); + typename Derived::Scalar r = ypr(2); + c = cos(r); + s = sin(r); + Rx(1, 1) = c; + Rx(2, 1) = s; + Rx(1, 2) = -s; + Rx(2, 2) = c; + Rx(0, 0) = 1; + + Eigen::Matrix R = Rz * Ry * Rx; + return R; +} + +template +Eigen::Matrix R_to_ypr(const Eigen::DenseBase& R) { + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); + EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + + Eigen::Matrix n = R.col(0); + Eigen::Matrix o = R.col(1); + Eigen::Matrix a = R.col(2); + + Eigen::Matrix ypr(3); + typename Derived::Scalar y = atan2(n(1), n(0)); + typename Derived::Scalar p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); + typename Derived::Scalar r = + atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); + ypr(0) = y; + ypr(1) = p; + ypr(2) = r; + + return ypr; +} + +template +Eigen::Quaternion ypr_to_quaternion(const Eigen::DenseBase& ypr) { + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); + EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + + const typename Derived::Scalar cy = cos(ypr(0) / 2.0); + const typename Derived::Scalar sy = sin(ypr(0) / 2.0); + const typename Derived::Scalar cp = cos(ypr(1) / 2.0); + const typename Derived::Scalar sp = sin(ypr(1) / 2.0); + const typename Derived::Scalar cr = cos(ypr(2) / 2.0); + const typename Derived::Scalar sr = sin(ypr(2) / 2.0); + + Eigen::Quaternion q; + + q.w() = cr * cp * cy + sr * sp * sy; + q.x() = sr * cp * cy - cr * sp * sy; + q.y() = cr * sp * cy + sr * cp * sy; + q.z() = cr * cp * sy - sr * sp * cy; + + return q; +} + +template +Eigen::Matrix quaternion_to_ypr(const Eigen::Quaternion& q_) { + Eigen::Quaternion q = q_.normalized(); + + Eigen::Matrix ypr; + ypr(2) = atan2(2 * (q.w() * q.x() + q.y() * q.z()), 1 - 2 * (q.x() * q.x() + q.y() * q.y())); + ypr(1) = asin(2 * (q.w() * q.y() - q.z() * q.x())); + ypr(0) = atan2(2 * (q.w() * q.z() + q.x() * q.y()), 1 - 2 * (q.y() * q.y() + q.z() * q.z())); + + return ypr; +} + +template +Scalar_t get_yaw_from_quaternion(const Eigen::Quaternion& q) { + return quaternion_to_ypr(q)(0); +} + +template +Eigen::Quaternion yaw_to_quaternion(Scalar_t yaw) { + return Eigen::Quaternion(rotz(yaw)); +} + +template +Scalar_t normalize_angle(Scalar_t a) { + int cnt = 0; + while (true) { + cnt++; + + if (a < -M_PI) { + a += M_PI * 2.0; + } else if (a > M_PI) { + a -= M_PI * 2.0; + } + + if (-M_PI <= a && a <= M_PI) { + break; + }; + + assert(cnt < 10 && "[uav_utils/geometry_msgs] INVALID INPUT ANGLE"); + } + + return a; +} + +template +Scalar_t angle_add(Scalar_t a, Scalar_t b) { + Scalar_t c = a + b; + c = normalize_angle(c); + assert(-M_PI <= c && c <= M_PI); + return c; +} + +template +Scalar_t yaw_add(Scalar_t a, Scalar_t b) { + return angle_add(a, b); +} + +template +Eigen::Matrix get_skew_symmetric(const Eigen::DenseBase& v) { + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); + EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + + Eigen::Matrix M; + M.setZero(); + M(0, 1) = -v(2); + M(0, 2) = v(1); + M(1, 0) = v(2); + M(1, 2) = -v(0); + M(2, 0) = -v(1); + M(2, 1) = v(0); + return M; +} + +template +Eigen::Matrix from_skew_symmetric(const Eigen::DenseBase& M) { + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); + EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE); + + Eigen::Matrix v; + v(0) = M(2, 1); + v(1) = -M(2, 0); + v(2) = M(1, 0); + + assert(v.isApprox(Eigen::Matrix(-M(1, 2), M(0, 2), -M(0, 1)))); + + return v; +} + + +} // end of namespace uav_utils +/* clang-format on */ + +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/include/uav_utils/utils.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/include/uav_utils/utils.h new file mode 100644 index 0000000..0ce4c28 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/include/uav_utils/utils.h @@ -0,0 +1,59 @@ +#ifndef __UAV_UTILS_H +#define __UAV_UTILS_H + +#include + +#include +#include + +namespace uav_utils +{ + +/* judge if value belongs to [low,high] */ +template +bool +in_range(T value, const T2& low, const T2& high) +{ + ROS_ASSERT_MSG(low < high, "%f < %f?", low, high); + return (low <= value) && (value <= high); +} + +/* judge if value belongs to [-limit, limit] */ +template +bool +in_range(T value, const T2& limit) +{ + ROS_ASSERT_MSG(limit > 0, "%f > 0?", limit); + return in_range(value, -limit, limit); +} + +template +void +limit_range(T& value, const T2& low, const T2& high) +{ + ROS_ASSERT_MSG(low < high, "%f < %f?", low, high); + if (value < low) + { + value = low; + } + + if (value > high) + { + value = high; + } + + return; +} + +template +void +limit_range(T& value, const T2& limit) +{ + ROS_ASSERT_MSG(limit > 0, "%f > 0?", limit); + limit_range(value, -limit, limit); +} + +typedef std::stringstream DebugSS_t; +} // end of namespace uav_utils + +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/package.xml b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/package.xml new file mode 100644 index 0000000..db864b7 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/package.xml @@ -0,0 +1,58 @@ + + + uav_utils + 0.0.0 + The uav_utils package + + + + + LIU Tianbo + + + + + + LGPLv3 + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + cmake_modules + cmake_utils + roscpp + rospy + cmake_modules + cmake_utils + + + + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/odom_to_euler.py b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/odom_to_euler.py new file mode 100755 index 0000000..92ac879 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/odom_to_euler.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python + +import rospy +import numpy as np +import tf +from tf import transformations as tfs +from nav_msgs.msg import Odometry +from sensor_msgs.msg import Imu +from geometry_msgs.msg import Vector3Stamped +from sensor_msgs.msg import Joy + +pub = None +pub1 = None + +def callback(odom_msg): + q = np.array([odom_msg.pose.pose.orientation.x, + odom_msg.pose.pose.orientation.y, + odom_msg.pose.pose.orientation.z, + odom_msg.pose.pose.orientation.w]) + + e = tfs.euler_from_quaternion(q, 'rzyx') + + euler_msg = Vector3Stamped() + euler_msg.header = odom_msg.header + euler_msg.vector.z = e[0]*180.0/3.14159 + euler_msg.vector.y = e[1]*180.0/3.14159 + euler_msg.vector.x = e[2]*180.0/3.14159 + + pub.publish(euler_msg) + +def imu_callback(imu_msg): + q = np.array([imu_msg.orientation.x, + imu_msg.orientation.y, + imu_msg.orientation.z, + imu_msg.orientation.w]) + + e = tfs.euler_from_quaternion(q, 'rzyx') + + euler_msg = Vector3Stamped() + euler_msg.header = imu_msg.header + euler_msg.vector.z = e[0]*180.0/3.14159 + euler_msg.vector.y = e[1]*180.0/3.14159 + euler_msg.vector.x = e[2]*180.0/3.14159 + + pub1.publish(euler_msg) + +def joy_callback(joy_msg): + out_msg = Vector3Stamped() + out_msg.header = joy_msg.header + out_msg.vector.z = -joy_msg.axes[3] + out_msg.vector.y = joy_msg.axes[1] + out_msg.vector.x = joy_msg.axes[0] + + pub2.publish(out_msg) + + +if __name__ == "__main__": + rospy.init_node("odom_to_euler") + + pub = rospy.Publisher("~euler", Vector3Stamped, queue_size=10) + sub = rospy.Subscriber("~odom", Odometry, callback) + + pub1 = rospy.Publisher("~imueuler", Vector3Stamped, queue_size=10) + sub1 = rospy.Subscriber("~imu", Imu, imu_callback) + + pub2 = rospy.Publisher("~ctrlout", Vector3Stamped, queue_size=10) + sub2 = rospy.Subscriber("~ctrlin", Joy, joy_callback) + + rospy.spin() diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/send_odom.py b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/send_odom.py new file mode 100755 index 0000000..a08b586 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/send_odom.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python + +import rospy +import numpy as np +import tf +from tf import transformations as tfs +from nav_msgs.msg import Odometry + +if __name__ == "__main__": + rospy.init_node("odom_sender") + + msg = Odometry() + + msg.header.stamp = rospy.Time.now()-rospy.Duration(0.2) + msg.header.frame_id = "world" + + q = tfs.quaternion_from_euler(0,0,0,"rzyx") + + msg.pose.pose.position.x = 0 + msg.pose.pose.position.y = 0 + msg.pose.pose.position.z = 0 + msg.twist.twist.linear.x = 0 + msg.twist.twist.linear.y = 0 + msg.twist.twist.linear.z = 0 + msg.pose.pose.orientation.x = q[0] + msg.pose.pose.orientation.y = q[1] + msg.pose.pose.orientation.z = q[2] + msg.pose.pose.orientation.w = q[3] + + print(msg) + + pub = rospy.Publisher("odom", Odometry, queue_size=10) + + counter = 0 + r = rospy.Rate(1) + + while not rospy.is_shutdown(): + counter += 1 + msg.header.stamp = rospy.Time.now()-rospy.Duration(0.2) + pub.publish(msg) + rospy.loginfo("Send %3d msg(s)."%counter) + r.sleep() diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/tf_assist.py b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/tf_assist.py new file mode 100755 index 0000000..259e8a1 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/tf_assist.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python + +import rospy +import numpy as np +import tf +from tf import transformations as tfs +from math import pi +from nav_msgs.msg import Odometry +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import Imu +from sensor_msgs.msg import Joy + + +imu_pub = None +odom_pub = None +br = None + + +class OdometryConverter(object): + + def __init__(self, frame_id_in_, frame_id_out_, broadcast_tf_, body_frame_id_, intermediate_frame_id_, world_frame_id_): + self.frame_id_in = frame_id_in_ + self.frame_id_out = frame_id_out_ + self.broadcast_tf = broadcast_tf_ + self.body_frame_id = body_frame_id_ + self.intermediate_frame_id = intermediate_frame_id_ + self.world_frame_id = world_frame_id_ + self.in_odom_sub = None + self.out_odom_pub = None + self.out_path_pub = None + self.path_pub_timer = None + self.tf_pub_flag = True + if self.broadcast_tf: + rospy.loginfo('ROSTopic: [%s]->[%s] TF: [%s]-[%s]-[%s]', + self.frame_id_in, self.frame_id_out, self.body_frame_id, self.intermediate_frame_id, self.world_frame_id) + else: + rospy.loginfo('ROSTopic: [%s]->[%s] No TF', + self.frame_id_in, self.frame_id_out) + + self.path = [] + + def in_odom_callback(self, in_odom_msg): + q = np.array([in_odom_msg.pose.pose.orientation.x, + in_odom_msg.pose.pose.orientation.y, + in_odom_msg.pose.pose.orientation.z, + in_odom_msg.pose.pose.orientation.w]) + p = np.array([in_odom_msg.pose.pose.position.x, + in_odom_msg.pose.pose.position.y, + in_odom_msg.pose.pose.position.z]) + + e = tfs.euler_from_quaternion(q, 'rzyx') + wqb = tfs.quaternion_from_euler(e[0], e[1], e[2], 'rzyx') + wqc = tfs.quaternion_from_euler(e[0], 0.0, 0.0, 'rzyx') + + #### odom #### + odom_msg = in_odom_msg + assert(in_odom_msg.header.frame_id == self.frame_id_in) + odom_msg.header.frame_id = self.frame_id_out + odom_msg.child_frame_id = "" + self.out_odom_pub.publish(odom_msg) + + #### tf #### + if self.broadcast_tf and self.tf_pub_flag: + self.tf_pub_flag = False + if not self.frame_id_in == self.frame_id_out: + br.sendTransform((0.0, 0.0, 0.0), + tfs.quaternion_from_euler(0.0, 0.0, 0.0, 'rzyx'), + odom_msg.header.stamp, + self.frame_id_in, + self.frame_id_out) + + if not self.world_frame_id == self.frame_id_out: + br.sendTransform((0.0, 0.0, 0.0), + tfs.quaternion_from_euler(0.0, 0.0, 0.0, 'rzyx'), + odom_msg.header.stamp, + self.world_frame_id, + self.frame_id_out) + + br.sendTransform((p[0], p[1], p[2]), + wqb, + odom_msg.header.stamp, + self.body_frame_id, + self.world_frame_id) + + br.sendTransform(((p[0], p[1], p[2])), + wqc, + odom_msg.header.stamp, + self.intermediate_frame_id, + self.world_frame_id) + #### path #### + pose = PoseStamped() + pose.header = odom_msg.header + pose.pose.position.x = p[0] + pose.pose.position.y = p[1] + pose.pose.position.z = p[2] + pose.pose.orientation.x = q[0] + pose.pose.orientation.y = q[1] + pose.pose.orientation.z = q[2] + pose.pose.orientation.w = q[3] + + self.path.append(pose) + + def path_pub_callback(self, event): + if self.path: + path = Path() + path.header = self.path[-1].header + path.poses = self.path[-30000::1] + self.out_path_pub.publish(path) + + def tf_pub_callback(self, event): + self.tf_pub_flag = True + + +if __name__ == "__main__": + rospy.init_node('tf_assist') + + converters = [] + index = 0 + while True: + prefix = "~converter%d/" % index + try: + frame_id_in = rospy.get_param('%sframe_id_in' % prefix) + frame_id_out = rospy.get_param('%sframe_id_out' % prefix) + broadcast_tf = rospy.get_param('%sbroadcast_tf' % prefix, False) + body_frame_id = rospy.get_param('%sbody_frame_id' % prefix, 'body') + intermediate_frame_id = rospy.get_param( + '%sintermediate_frame_id' % prefix, 'intermediate') + world_frame_id = rospy.get_param( + '%sworld_frame_id' % prefix, 'world') + + converter = OdometryConverter( + frame_id_in, frame_id_out, broadcast_tf, body_frame_id, intermediate_frame_id, world_frame_id) + converter.in_odom_sub = rospy.Subscriber( + '%sin_odom' % prefix, Odometry, converter.in_odom_callback, tcp_nodelay=True) + converter.out_odom_pub = rospy.Publisher( + '%sout_odom' % prefix, Odometry, queue_size=10, tcp_nodelay=True) + converter.out_path_pub = rospy.Publisher( + '%sout_path' % prefix, Path, queue_size=10) + + converter.tf_pub_timer = rospy.Timer( + rospy.Duration(0.1), converter.tf_pub_callback) + + converter.path_pub_timer = rospy.Timer( + rospy.Duration(0.5), converter.path_pub_callback) + + index += 1 + except KeyError, e: + if index == 0: + raise(KeyError(e)) + else: + if index == 1: + rospy.loginfo( + 'prefix:"%s" not found. Generate %d converter.' % (prefix, index)) + else: + rospy.loginfo( + 'prefix:"%s" not found. Generate %d converters' % (prefix, index)) + break + + br = tf.TransformBroadcaster() + + rospy.spin() diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/topic_statistics.py b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/topic_statistics.py new file mode 100755 index 0000000..c69ffb6 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/scripts/topic_statistics.py @@ -0,0 +1,15 @@ +#!/usr/bin/env python + +import argparse as ap +import argcomplete + +def main(**args): + pass + +if __name__ == '__main__': + parser = ap.ArgumentParser() + parser.add_argument('positional', choices=['spam', 'eggs']) + parser.add_argument('--optional', choices=['foo1', 'foo2', 'bar']) + argcomplete.autocomplete(parser) + args = parser.parse_args() + main(**vars(args)) \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/src/uav_utils_test.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/src/uav_utils_test.cpp new file mode 100644 index 0000000..60da5af --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/uav_utils/src/uav_utils_test.cpp @@ -0,0 +1,122 @@ +#include +#include + +using namespace uav_utils; +using namespace Eigen; + +#define DOUBLE_EPS 1.0e-15 +#define FLOAT_EPS 1.0e-6 + +TEST(GeometryUtilsDouble, Rotation) { + Vector3d v(0.1, 0.2, 0.3); + ASSERT_TRUE(v.isApprox(R_to_ypr(ypr_to_R(v)),DOUBLE_EPS)); + ASSERT_TRUE(v.isApprox(quaternion_to_ypr(ypr_to_quaternion(v)),DOUBLE_EPS)); + ASSERT_TRUE(v.isApprox(R_to_ypr(rotz(v(0)) * roty(v(1)) * rotx(v(2))),DOUBLE_EPS)); + + double yaw = 30.0; + ASSERT_DOUBLE_EQ(30.0, toDeg(get_yaw_from_quaternion(yaw_to_quaternion(toRad(yaw))))); +} + +TEST(GeometryUtilsFloat, Rotation) { + Vector3f v(0.1, 0.2, 0.3); + ASSERT_TRUE(v.isApprox(R_to_ypr(ypr_to_R(v)),FLOAT_EPS)); + ASSERT_TRUE(v.isApprox(quaternion_to_ypr(ypr_to_quaternion(v)),FLOAT_EPS)); + ASSERT_TRUE(v.isApprox(R_to_ypr(rotz(v(0)) * roty(v(1)) * rotx(v(2))),FLOAT_EPS)); + + float yaw = 30.0; + ASSERT_FLOAT_EQ(30.0, toDeg(get_yaw_from_quaternion(yaw_to_quaternion(toRad(yaw))))); +} + +TEST(GeometryUtilsDouble, Skew) { + double v1 = 0.1; + double v2 = 0.2; + double v3 = 0.3; + Vector3d v(v1, v2, v3); + Matrix3d M; + M << .0, -v3, v2, + v3, .0, -v1, + -v2, v1, .0; + + ASSERT_TRUE(M.isApprox(get_skew_symmetric(v), DOUBLE_EPS)); + ASSERT_TRUE(v.isApprox(from_skew_symmetric(M), DOUBLE_EPS)); +} + +TEST(GeometryUtilsFloat, Skew) { + float v1 = 0.1; + float v2 = 0.2; + float v3 = 0.3; + Vector3f v(v1, v2, v3); + Matrix3f M; + M << .0, -v3, v2, + v3, .0, -v1, + -v2, v1, .0; + + ASSERT_TRUE(M.isApprox(get_skew_symmetric(v), FLOAT_EPS)); + ASSERT_TRUE(v.isApprox(from_skew_symmetric(M), FLOAT_EPS)); +} + +TEST(GeometryUtilsDouble, Angle) { + double a = toRad(179.0); + double b = toRad(2.0); + ASSERT_DOUBLE_EQ(-179.0, toDeg(angle_add(a, b))); + ASSERT_DOUBLE_EQ(-179.0, toDeg(yaw_add(a, b))); + ASSERT_DOUBLE_EQ(179.0, toDeg(angle_add(-a, -b))); + ASSERT_DOUBLE_EQ(179.0, toDeg(yaw_add(-a, -b))); + ASSERT_DOUBLE_EQ(177.0, toDeg(angle_add(a, -b))); + ASSERT_DOUBLE_EQ(177.0, toDeg(yaw_add(a, -b))); + ASSERT_NEAR(toRad(-2.0), normalize_angle(toRad(358.0)), DOUBLE_EPS); +} + +TEST(GeometryUtilsFloat, Angle) { + float a = toRad(179.0); + float b = toRad(2.0); + ASSERT_FLOAT_EQ(-179.0, toDeg(angle_add(a, b))); + ASSERT_FLOAT_EQ(-179.0, toDeg(yaw_add(a, b))); + ASSERT_FLOAT_EQ(179.0, toDeg(angle_add(-a, -b))); + ASSERT_FLOAT_EQ(179.0, toDeg(yaw_add(-a, -b))); + ASSERT_FLOAT_EQ(177.0, toDeg(angle_add(a, -b))); + ASSERT_FLOAT_EQ(177.0, toDeg(yaw_add(a, -b))); + ASSERT_NEAR(-2.0, toDeg(normalize_angle(toRad(358.0))),FLOAT_EPS); +} + +TEST(ConverterDouble, Equality) { + nav_msgs::OdometryPtr pOdom(new nav_msgs::Odometry()); + + pOdom->pose.pose.position.x = 1.0; + pOdom->pose.pose.position.y = 2.0; + pOdom->pose.pose.position.z = 3.0; + + pOdom->pose.pose.orientation.w = 0.5; + pOdom->pose.pose.orientation.x = -0.5; + pOdom->pose.pose.orientation.y = 0.5; + pOdom->pose.pose.orientation.z = -0.5; + + pOdom->twist.twist.linear.x = -1.0; + pOdom->twist.twist.linear.y = -2.0; + pOdom->twist.twist.linear.z = -3.0; + + pOdom->twist.twist.angular.x = -0.1; + pOdom->twist.twist.angular.y = -0.2; + pOdom->twist.twist.angular.z = -0.3; + + Eigen::Vector3d p, v, w; + Eigen::Quaterniond q; + + nav_msgs::Odometry odom_ = *pOdom; + + extract_odometry(pOdom, p, v, q, w); + + ASSERT_TRUE(v.isApprox(from_vector3_msg(pOdom->twist.twist.linear))); + ASSERT_TRUE(w.isApprox(from_vector3_msg(pOdom->twist.twist.angular))); + ASSERT_TRUE(p.isApprox(from_point_msg(pOdom->pose.pose.position))); + ASSERT_TRUE(q.isApprox(from_quaternion_msg(pOdom->pose.pose.orientation))); + + ASSERT_TRUE(v.isApprox(from_vector3_msg(to_vector3_msg(v)))); + ASSERT_TRUE(p.isApprox(from_point_msg(to_point_msg(p)))); + ASSERT_TRUE(q.isApprox(from_quaternion_msg(to_quaternion_msg(q)))); +} + +int main(int argc, char* argv[]) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/CMakeLists.txt new file mode 100755 index 0000000..eb4e752 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 2.8.3) +project(waypoint_generator) + +set(CMAKE_VERBOSE_MAKEFILE "false") +include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) +if(COMPILER_SUPPORTS_CXX11) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +elseif(COMPILER_SUPPORTS_CXX0X) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +else() + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") +endif() + +set(ADDITIONAL_CXX_FLAG "-Wall -O3 -march=native") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CXX_FLAG}") + +find_package(catkin REQUIRED COMPONENTS + roscpp + tf + nav_msgs +) +catkin_package() + +include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_executable(waypoint_generator src/waypoint_generator.cpp) + +target_link_libraries(waypoint_generator + ${catkin_LIBRARIES} +) diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/package.xml b/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/package.xml new file mode 100755 index 0000000..2c6078d --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/package.xml @@ -0,0 +1,25 @@ + + 0.0.0 + waypoint_generator + + + waypoint_generator + + + Shaojie Shen + BSD + http://ros.org/wiki/waypoint_generator + + catkin + + roscpp + tf + nav_msgs + + roscpp + tf + nav_msgs + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/src/sample_waypoints.h b/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/src/sample_waypoints.h new file mode 100755 index 0000000..8daad3c --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/src/sample_waypoints.h @@ -0,0 +1,213 @@ +#ifndef SAMPLE_WAYPOINTS_H +#define SAMPLE_WAYPOINTS_H + +#include +#include +#include + +nav_msgs::Path point() +{ + // Circle parameters + nav_msgs::Path waypoints; + geometry_msgs::PoseStamped pt; + pt.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); + + double h = 1.0; + double scale = 7.0; + + pt.pose.position.y = scale * 0.0; + pt.pose.position.x = scale * 2.0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = scale * 0.0; + pt.pose.position.x = scale * 4.0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = scale * 0.25; + pt.pose.position.x = scale * 5.0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = scale * 0.5; + pt.pose.position.x = scale * 5.3; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = scale * 0.75; + pt.pose.position.x = scale * 5.0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = scale * 1.0; + pt.pose.position.x = scale * 4.0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = scale * 1.0; + pt.pose.position.x = scale * 2.0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = scale * 1.0; + pt.pose.position.x = scale * 0.0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + // Return + return waypoints; +} + +// Circle trajectory +nav_msgs::Path circle() +{ + double h = 1.0; + double scale = 5.0; + nav_msgs::Path waypoints; + geometry_msgs::PoseStamped pt; + pt.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); + + pt.pose.position.y = -1.2 * scale; + pt.pose.position.x = 2.5 * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = -2.4 * scale; + pt.pose.position.x = 5.0 * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + pt.pose.position.y = 0.0 * scale; + pt.pose.position.x = 5.0 * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = -1.2 * scale; + pt.pose.position.x = 2.5 * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = -2.4 * scale; + pt.pose.position.x = 0. * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + pt.pose.position.y = 0.0 * scale; + pt.pose.position.x = 0.0 * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = -1.2 * scale; + pt.pose.position.x = 2.5 * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = -2.4 * scale; + pt.pose.position.x = 5.0 * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + pt.pose.position.y = 0.0 * scale; + pt.pose.position.x = 5.0 * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = -1.2 * scale; + pt.pose.position.x = 2.5 * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + pt.pose.position.y = -2.4 * scale; + pt.pose.position.x = 0. * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + pt.pose.position.y = 0.0 * scale; + pt.pose.position.x = 0.0 * scale; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + + // Return + return waypoints; +} + +// Figure 8 trajectory +nav_msgs::Path eight() +{ + // Circle parameters + double offset_x = 0.0; + double offset_y = 0.0; + double r = 10.0; + double h = 2.0; + nav_msgs::Path waypoints; + geometry_msgs::PoseStamped pt; + pt.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); + + for(int i=0; i< 1; ++i) + { + // First loop + pt.pose.position.x = r + offset_x; + pt.pose.position.y = -r + offset_y; + pt.pose.position.z = h/2; + waypoints.poses.push_back(pt); + pt.pose.position.x = r*2 + offset_x * 2; + pt.pose.position.y = 0 ; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + pt.pose.position.x = r*3 + offset_x * 3; + pt.pose.position.y = r ; + pt.pose.position.z = h/2; + waypoints.poses.push_back(pt); + pt.pose.position.x = r*4 + offset_x * 4; + pt.pose.position.y = 0 ; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + pt.pose.position.x = r*3 + offset_x * 3; + pt.pose.position.y = -r ; + pt.pose.position.z = h/2; + waypoints.poses.push_back(pt); + pt.pose.position.x = r*2 + offset_x * 2; + pt.pose.position.y = 0 ; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + pt.pose.position.x = r + offset_x * 2; + pt.pose.position.y = r ; + pt.pose.position.z = h/2; + waypoints.poses.push_back(pt); + pt.pose.position.x = 0 + offset_x; + pt.pose.position.y = 0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + // Second loop + pt.pose.position.x = r + offset_x; + pt.pose.position.y = -r; + pt.pose.position.z = h / 2 * 3; + waypoints.poses.push_back(pt); + pt.pose.position.x = r*2 + offset_x * 2; + pt.pose.position.y = 0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + pt.pose.position.x = r*3 + offset_x * 3; + pt.pose.position.y = r; + pt.pose.position.z = h / 2 * 3; + waypoints.poses.push_back(pt); + pt.pose.position.x = r*4 + offset_x * 4; + pt.pose.position.y = 0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + pt.pose.position.x = r*3 + offset_x * 3; + pt.pose.position.y = -r; + pt.pose.position.z = h / 2 * 3; + waypoints.poses.push_back(pt); + pt.pose.position.x = r*2 + offset_x * 2; + pt.pose.position.y = 0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + pt.pose.position.x = r + offset_x; + pt.pose.position.y = r + offset_y; + pt.pose.position.z = h / 2 * 3; + waypoints.poses.push_back(pt); + pt.pose.position.x = 0; + pt.pose.position.y = 0; + pt.pose.position.z = h; + waypoints.poses.push_back(pt); + } + return waypoints; +} +#endif \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp b/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp new file mode 100755 index 0000000..1eb2d04 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/Utils/waypoint_generator/src/waypoint_generator.cpp @@ -0,0 +1,258 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include "sample_waypoints.h" +#include +#include +#include +#include + +using namespace std; +using bfmt = boost::format; + +ros::Publisher pub1; +ros::Publisher pub2; +ros::Publisher pub3; +string waypoint_type = string("manual"); +bool is_odom_ready; +nav_msgs::Odometry odom; +nav_msgs::Path waypoints; + +// series waypoint needed +std::deque waypointSegments; +ros::Time trigged_time; + +void load_seg(ros::NodeHandle& nh, int segid, const ros::Time& time_base) { + std::string seg_str = boost::str(bfmt("seg%d/") % segid); + double yaw; + double time_of_start = 0.0; + ROS_INFO("Getting segment %d", segid); + ROS_ASSERT(nh.getParam(seg_str + "yaw", yaw)); + ROS_ASSERT_MSG((yaw > -3.1499999) && (yaw < 3.14999999), "yaw=%.3f", yaw); + ROS_ASSERT(nh.getParam(seg_str + "time_of_start", time_of_start)); + ROS_ASSERT(time_of_start >= 0.0); + + std::vector ptx; + std::vector pty; + std::vector ptz; + + ROS_ASSERT(nh.getParam(seg_str + "x", ptx)); + ROS_ASSERT(nh.getParam(seg_str + "y", pty)); + ROS_ASSERT(nh.getParam(seg_str + "z", ptz)); + + ROS_ASSERT(ptx.size()); + ROS_ASSERT(ptx.size() == pty.size() && ptx.size() == ptz.size()); + + nav_msgs::Path path_msg; + + path_msg.header.stamp = time_base + ros::Duration(time_of_start); + + double baseyaw = tf::getYaw(odom.pose.pose.orientation); + + for (size_t k = 0; k < ptx.size(); ++k) { + geometry_msgs::PoseStamped pt; + pt.pose.orientation = tf::createQuaternionMsgFromYaw(baseyaw + yaw); + Eigen::Vector2d dp(ptx.at(k), pty.at(k)); + Eigen::Vector2d rdp; + rdp.x() = std::cos(-baseyaw-yaw)*dp.x() + std::sin(-baseyaw-yaw)*dp.y(); + rdp.y() =-std::sin(-baseyaw-yaw)*dp.x() + std::cos(-baseyaw-yaw)*dp.y(); + pt.pose.position.x = rdp.x() + odom.pose.pose.position.x; + pt.pose.position.y = rdp.y() + odom.pose.pose.position.y; + pt.pose.position.z = ptz.at(k) + odom.pose.pose.position.z; + path_msg.poses.push_back(pt); + } + + waypointSegments.push_back(path_msg); +} + +void load_waypoints(ros::NodeHandle& nh, const ros::Time& time_base) { + int seg_cnt = 0; + waypointSegments.clear(); + ROS_ASSERT(nh.getParam("segment_cnt", seg_cnt)); + for (int i = 0; i < seg_cnt; ++i) { + load_seg(nh, i, time_base); + if (i > 0) { + ROS_ASSERT(waypointSegments[i - 1].header.stamp < waypointSegments[i].header.stamp); + } + } + ROS_INFO("Overall load %zu segments", waypointSegments.size()); +} + +void publish_waypoints() { + waypoints.header.frame_id = std::string("world"); + waypoints.header.stamp = ros::Time::now(); + pub1.publish(waypoints); + geometry_msgs::PoseStamped init_pose; + init_pose.header = odom.header; + init_pose.pose = odom.pose.pose; + waypoints.poses.insert(waypoints.poses.begin(), init_pose); + // pub2.publish(waypoints); + waypoints.poses.clear(); +} + +void publish_waypoints_vis() { + nav_msgs::Path wp_vis = waypoints; + geometry_msgs::PoseArray poseArray; + poseArray.header.frame_id = std::string("world"); + poseArray.header.stamp = ros::Time::now(); + + { + geometry_msgs::Pose init_pose; + init_pose = odom.pose.pose; + poseArray.poses.push_back(init_pose); + } + + for (auto it = waypoints.poses.begin(); it != waypoints.poses.end(); ++it) { + geometry_msgs::Pose p; + p = it->pose; + poseArray.poses.push_back(p); + } + pub2.publish(poseArray); +} + +void odom_callback(const nav_msgs::Odometry::ConstPtr& msg) { + is_odom_ready = true; + odom = *msg; + + if (waypointSegments.size()) { + ros::Time expected_time = waypointSegments.front().header.stamp; + if (odom.header.stamp >= expected_time) { + waypoints = waypointSegments.front(); + + std::stringstream ss; + ss << bfmt("Series send %.3f from start:\n") % trigged_time.toSec(); + for (auto& pose_stamped : waypoints.poses) { + ss << bfmt("P[%.2f, %.2f, %.2f] q(%.2f,%.2f,%.2f,%.2f)") % + pose_stamped.pose.position.x % pose_stamped.pose.position.y % + pose_stamped.pose.position.z % pose_stamped.pose.orientation.w % + pose_stamped.pose.orientation.x % pose_stamped.pose.orientation.y % + pose_stamped.pose.orientation.z << std::endl; + } + ROS_INFO_STREAM(ss.str()); + + publish_waypoints_vis(); + publish_waypoints(); + + waypointSegments.pop_front(); + } + } +} + +void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) { +/* if (!is_odom_ready) { + ROS_ERROR("[waypoint_generator] No odom!"); + return; + }*/ + + trigged_time = ros::Time::now(); //odom.header.stamp; + //ROS_ASSERT(trigged_time > ros::Time(0)); + + ros::NodeHandle n("~"); + n.param("waypoint_type", waypoint_type, string("manual")); + + if (waypoint_type == string("circle")) { + waypoints = circle(); + publish_waypoints_vis(); + publish_waypoints(); + } else if (waypoint_type == string("eight")) { + waypoints = eight(); + publish_waypoints_vis(); + publish_waypoints(); + } else if (waypoint_type == string("points")) { + waypoints = point(); + publish_waypoints_vis(); + publish_waypoints(); + } else if (waypoint_type == string("series")) { + load_waypoints(n, trigged_time); + } else if (waypoint_type == string("manual-lonely-waypoint")) { + if (msg->pose.position.z > -0.1) { + // if height > 0, it's a valid goal; + geometry_msgs::PoseStamped pt = *msg; + waypoints.poses.clear(); + waypoints.poses.push_back(pt); + publish_waypoints_vis(); + publish_waypoints(); + } else { + ROS_WARN("[waypoint_generator] invalid goal in manual-lonely-waypoint mode."); + } + } else { + if (msg->pose.position.z > 0) { + // if height > 0, it's a normal goal; + geometry_msgs::PoseStamped pt = *msg; + if (waypoint_type == string("noyaw")) { + double yaw = tf::getYaw(odom.pose.pose.orientation); + pt.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); + } + waypoints.poses.push_back(pt); + publish_waypoints_vis(); + } else if (msg->pose.position.z > -1.0) { + // if 0 > height > -1.0, remove last goal; + if (waypoints.poses.size() >= 1) { + waypoints.poses.erase(std::prev(waypoints.poses.end())); + } + publish_waypoints_vis(); + } else { + // if -1.0 > height, end of input + if (waypoints.poses.size() >= 1) { + publish_waypoints_vis(); + publish_waypoints(); + } + } + } +} + +void traj_start_trigger_callback(const geometry_msgs::PoseStamped& msg) { + if (!is_odom_ready) { + ROS_ERROR("[waypoint_generator] No odom!"); + return; + } + + ROS_WARN("[waypoint_generator] Trigger!"); + trigged_time = odom.header.stamp; + ROS_ASSERT(trigged_time > ros::Time(0)); + + ros::NodeHandle n("~"); + n.param("waypoint_type", waypoint_type, string("manual")); + + ROS_ERROR_STREAM("Pattern " << waypoint_type << " generated!"); + if (waypoint_type == string("free")) { + waypoints = point(); + publish_waypoints_vis(); + publish_waypoints(); + } else if (waypoint_type == string("circle")) { + waypoints = circle(); + publish_waypoints_vis(); + publish_waypoints(); + } else if (waypoint_type == string("eight")) { + waypoints = eight(); + publish_waypoints_vis(); + publish_waypoints(); + } else if (waypoint_type == string("point")) { + waypoints = point(); + publish_waypoints_vis(); + publish_waypoints(); + } else if (waypoint_type == string("series")) { + load_waypoints(n, trigged_time); + } +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "waypoint_generator"); + ros::NodeHandle n("~"); + n.param("waypoint_type", waypoint_type, string("manual")); + ros::Subscriber sub1 = n.subscribe("odom", 10, odom_callback); + ros::Subscriber sub2 = n.subscribe("goal", 10, goal_callback); + ros::Subscriber sub3 = n.subscribe("traj_start_trigger", 10, traj_start_trigger_callback); + pub1 = n.advertise("waypoints", 50); + pub2 = n.advertise("waypoints_vis", 10); + + trigged_time = ros::Time(0); + + ros::spin(); + return 0; +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/fake_drone/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/fake_drone/CMakeLists.txt new file mode 100644 index 0000000..3ab9704 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/fake_drone/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 2.8.3) +project(poscmd_2_odom) + +set(CMAKE_BUILD_TYPE "Release") +#set(CMAKE_CXX_FLAGS "-std=c++11") +ADD_COMPILE_OPTIONS(-std=c++11 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + nav_msgs + quadrotor_msgs +) +find_package(Eigen3 REQUIRED) + +catkin_package() + +include_directories( +# include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +add_executable (poscmd_2_odom src/poscmd_2_odom.cpp ) +target_link_libraries(poscmd_2_odom + ${catkin_LIBRARIES}) diff --git a/motion_planning/3d/ego_planner/uav_simulator/fake_drone/package.xml b/motion_planning/3d/ego_planner/uav_simulator/fake_drone/package.xml new file mode 100644 index 0000000..145cd10 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/fake_drone/package.xml @@ -0,0 +1,68 @@ + + + poscmd_2_odom + 0.0.0 + The poscmd_2_odom package + + + + + todo + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + nav_msgs + quadrotor_msgs + roscpp + nav_msgs + quadrotor_msgs + roscpp + nav_msgs + quadrotor_msgs + + + + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/fake_drone/src/poscmd_2_odom.cpp b/motion_planning/3d/ego_planner/uav_simulator/fake_drone/src/poscmd_2_odom.cpp new file mode 100644 index 0000000..104433d --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/fake_drone/src/poscmd_2_odom.cpp @@ -0,0 +1,104 @@ +#include +#include +#include +#include +#include +#include +#include "quadrotor_msgs/PositionCommand.h" + +ros::Subscriber _cmd_sub; +ros::Publisher _odom_pub; + +quadrotor_msgs::PositionCommand _cmd; +double _init_x, _init_y, _init_z; + +bool rcv_cmd = false; +void rcvPosCmdCallBack(const quadrotor_msgs::PositionCommand cmd) +{ + rcv_cmd = true; + _cmd = cmd; +} + +void pubOdom() +{ + nav_msgs::Odometry odom; + odom.header.stamp = ros::Time::now(); + odom.header.frame_id = "world"; + + if(rcv_cmd) + { + odom.pose.pose.position.x = _cmd.position.x; + odom.pose.pose.position.y = _cmd.position.y; + odom.pose.pose.position.z = _cmd.position.z; + + Eigen::Vector3d alpha = Eigen::Vector3d(_cmd.acceleration.x, _cmd.acceleration.y, _cmd.acceleration.z) + 9.8*Eigen::Vector3d(0,0,1); + Eigen::Vector3d xC(cos(_cmd.yaw), sin(_cmd.yaw), 0); + Eigen::Vector3d yC(-sin(_cmd.yaw), cos(_cmd.yaw), 0); + Eigen::Vector3d xB = (yC.cross(alpha)).normalized(); + Eigen::Vector3d yB = (alpha.cross(xB)).normalized(); + Eigen::Vector3d zB = xB.cross(yB); + Eigen::Matrix3d R; + R.col(0) = xB; + R.col(1) = yB; + R.col(2) = zB; + Eigen::Quaterniond q(R); + odom.pose.pose.orientation.w = q.w(); + odom.pose.pose.orientation.x = q.x(); + odom.pose.pose.orientation.y = q.y(); + odom.pose.pose.orientation.z = q.z(); + + odom.twist.twist.linear.x = _cmd.velocity.x; + odom.twist.twist.linear.y = _cmd.velocity.y; + odom.twist.twist.linear.z = _cmd.velocity.z; + + odom.twist.twist.angular.x = _cmd.acceleration.x; + odom.twist.twist.angular.y = _cmd.acceleration.y; + odom.twist.twist.angular.z = _cmd.acceleration.z; + } + else + { + odom.pose.pose.position.x = _init_x; + odom.pose.pose.position.y = _init_y; + odom.pose.pose.position.z = _init_z; + + odom.pose.pose.orientation.w = 1; + odom.pose.pose.orientation.x = 0; + odom.pose.pose.orientation.y = 0; + odom.pose.pose.orientation.z = 0; + + odom.twist.twist.linear.x = 0.0; + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.linear.z = 0.0; + + odom.twist.twist.angular.x = 0.0; + odom.twist.twist.angular.y = 0.0; + odom.twist.twist.angular.z = 0.0; + } + + _odom_pub.publish(odom); +} + +int main (int argc, char** argv) +{ + ros::init (argc, argv, "odom_generator"); + ros::NodeHandle nh( "~" ); + + nh.param("init_x", _init_x, 0.0); + nh.param("init_y", _init_y, 0.0); + nh.param("init_z", _init_z, 0.0); + + _cmd_sub = nh.subscribe( "command", 1, rcvPosCmdCallBack ); + _odom_pub = nh.advertise("odometry", 1); + + ros::Rate rate(100); + bool status = ros::ok(); + while(status) + { + pubOdom(); + ros::spinOnce(); + status = ros::ok(); + rate.sleep(); + } + + return 0; +} \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeLists.txt new file mode 100644 index 0000000..13c9911 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeLists.txt @@ -0,0 +1,92 @@ +PROJECT(local_sensing_node) +CMAKE_MINIMUM_REQUIRED(VERSION 2.8.3) +SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo +#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +ADD_COMPILE_OPTIONS(-std=c++11 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) + +#set(ENABLE_CUDA false) +set(ENABLE_CUDA false) + +if(ENABLE_CUDA) + find_package(CUDA REQUIRED) + SET(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-O3 -use_fast_math) + set(CUDA_NVCC_FLAGS +# -gencode arch=compute_20,code=sm_20; +# -gencode arch=compute_20,code=sm_21; +# -gencode arch=compute_30,code=sm_30; +# -gencode arch=compute_35,code=sm_35; +# -gencode arch=compute_50,code=sm_50; +# -gencode arch=compute_52,code=sm_52; +# -gencode arch=compute_60,code=sm_60; +# -gencode arch=compute_61,code=sm_61; + -gencode arch=compute_75,code=sm_75; + ) + + SET(CUDA_PROPAGATE_HOST_FLAGS OFF) + + find_package(OpenCV REQUIRED) + find_package(Eigen3 REQUIRED) + find_package(Boost REQUIRED COMPONENTS system filesystem) + + find_package(catkin REQUIRED COMPONENTS + roscpp roslib cmake_modules cv_bridge image_transport pcl_ros sensor_msgs geometry_msgs nav_msgs quadrotor_msgs dynamic_reconfigure) + generate_dynamic_reconfigure_options( + cfg/local_sensing_node.cfg + ) + catkin_package( + DEPENDS OpenCV Eigen Boost + CATKIN_DEPENDS roscpp roslib image_transport pcl_ros + #INCLUDE_DIRS include + LIBRARIES depth_render_cuda + ) + + include_directories( + SYSTEM + #include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ) + + CUDA_ADD_LIBRARY( depth_render_cuda + src/depth_render.cu + ) + + add_executable( + pcl_render_node + src/pcl_render_node.cpp + ) + target_link_libraries( pcl_render_node + depth_render_cuda + ${OpenCV_LIBS} + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ) +else(ENABLE_CUDA) + find_package(Eigen3 REQUIRED) + find_package(catkin REQUIRED COMPONENTS + roscpp roslib cmake_modules pcl_ros sensor_msgs geometry_msgs nav_msgs quadrotor_msgs) + + catkin_package( + DEPENDS Eigen + CATKIN_DEPENDS roscpp roslib pcl_ros + ) + + include_directories( + SYSTEM + ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS}. + ) + + add_executable( + pcl_render_node + src/pointcloud_render_node.cpp + ) + + target_link_libraries( pcl_render_node + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + ) +endif(ENABLE_CUDA) diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA.cmake b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA.cmake new file mode 100644 index 0000000..8afb632 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA.cmake @@ -0,0 +1,1311 @@ +# - Tools for building CUDA C files: libraries and build dependencies. +# This script locates the NVIDIA CUDA C tools. It should work on linux, windows, +# and mac and should be reasonably up to date with CUDA C releases. +# +# This script makes use of the standard find_package arguments of , +# REQUIRED and QUIET. CUDA_FOUND will report if an acceptable version of CUDA +# was found. +# +# The script will prompt the user to specify CUDA_TOOLKIT_ROOT_DIR if the prefix +# cannot be determined by the location of nvcc in the system path and REQUIRED +# is specified to find_package(). To use a different installed version of the +# toolkit set the environment variable CUDA_BIN_PATH before running cmake +# (e.g. CUDA_BIN_PATH=/usr/local/cuda1.0 instead of the default /usr/local/cuda) +# or set CUDA_TOOLKIT_ROOT_DIR after configuring. If you change the value of +# CUDA_TOOLKIT_ROOT_DIR, various components that depend on the path will be +# relocated. +# +# It might be necessary to set CUDA_TOOLKIT_ROOT_DIR manually on certain +# platforms, or to use a cuda runtime not installed in the default location. In +# newer versions of the toolkit the cuda library is included with the graphics +# driver- be sure that the driver version matches what is needed by the cuda +# runtime version. +# +# The following variables affect the behavior of the macros in the script (in +# alphebetical order). Note that any of these flags can be changed multiple +# times in the same directory before calling CUDA_ADD_EXECUTABLE, +# CUDA_ADD_LIBRARY, CUDA_COMPILE, CUDA_COMPILE_PTX or CUDA_WRAP_SRCS. +# +# CUDA_64_BIT_DEVICE_CODE (Default matches host bit size) +# -- Set to ON to compile for 64 bit device code, OFF for 32 bit device code. +# Note that making this different from the host code when generating object +# or C files from CUDA code just won't work, because size_t gets defined by +# nvcc in the generated source. If you compile to PTX and then load the +# file yourself, you can mix bit sizes between device and host. +# +# CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE (Default ON) +# -- Set to ON if you want the custom build rule to be attached to the source +# file in Visual Studio. Turn OFF if you add the same cuda file to multiple +# targets. +# +# This allows the user to build the target from the CUDA file; however, bad +# things can happen if the CUDA source file is added to multiple targets. +# When performing parallel builds it is possible for the custom build +# command to be run more than once and in parallel causing cryptic build +# errors. VS runs the rules for every source file in the target, and a +# source can have only one rule no matter how many projects it is added to. +# When the rule is run from multiple targets race conditions can occur on +# the generated file. Eventually everything will get built, but if the user +# is unaware of this behavior, there may be confusion. It would be nice if +# this script could detect the reuse of source files across multiple targets +# and turn the option off for the user, but no good solution could be found. +# +# CUDA_BUILD_CUBIN (Default OFF) +# -- Set to ON to enable and extra compilation pass with the -cubin option in +# Device mode. The output is parsed and register, shared memory usage is +# printed during build. +# +# CUDA_BUILD_EMULATION (Default OFF for device mode) +# -- Set to ON for Emulation mode. -D_DEVICEEMU is defined for CUDA C files +# when CUDA_BUILD_EMULATION is TRUE. +# +# CUDA_GENERATED_OUTPUT_DIR (Default CMAKE_CURRENT_BINARY_DIR) +# -- Set to the path you wish to have the generated files placed. If it is +# blank output files will be placed in CMAKE_CURRENT_BINARY_DIR. +# Intermediate files will always be placed in +# CMAKE_CURRENT_BINARY_DIR/CMakeFiles. +# +# CUDA_HOST_COMPILATION_CPP (Default ON) +# -- Set to OFF for C compilation of host code. +# +# CUDA_NVCC_FLAGS +# CUDA_NVCC_FLAGS_ +# -- Additional NVCC command line arguments. NOTE: multiple arguments must be +# semi-colon delimited (e.g. --compiler-options;-Wall) +# +# CUDA_PROPAGATE_HOST_FLAGS (Default ON) +# -- Set to ON to propagate CMAKE_{C,CXX}_FLAGS and their configuration +# dependent counterparts (e.g. CMAKE_C_FLAGS_DEBUG) automatically to the +# host compiler through nvcc's -Xcompiler flag. This helps make the +# generated host code match the rest of the system better. Sometimes +# certain flags give nvcc problems, and this will help you turn the flag +# propagation off. This does not affect the flags supplied directly to nvcc +# via CUDA_NVCC_FLAGS or through the OPTION flags specified through +# CUDA_ADD_LIBRARY, CUDA_ADD_EXECUTABLE, or CUDA_WRAP_SRCS. Flags used for +# shared library compilation are not affected by this flag. +# +# CUDA_VERBOSE_BUILD (Default OFF) +# -- Set to ON to see all the commands used when building the CUDA file. When +# using a Makefile generator the value defaults to VERBOSE (run make +# VERBOSE=1 to see output), although setting CUDA_VERBOSE_BUILD to ON will +# always print the output. +# +# The script creates the following macros (in alphebetical order): +# +# CUDA_ADD_CUFFT_TO_TARGET( cuda_target ) +# -- Adds the cufft library to the target (can be any target). Handles whether +# you are in emulation mode or not. +# +# CUDA_ADD_CUBLAS_TO_TARGET( cuda_target ) +# -- Adds the cublas library to the target (can be any target). Handles +# whether you are in emulation mode or not. +# +# CUDA_ADD_EXECUTABLE( cuda_target file0 file1 ... +# [WIN32] [MACOSX_BUNDLE] [EXCLUDE_FROM_ALL] [OPTIONS ...] ) +# -- Creates an executable "cuda_target" which is made up of the files +# specified. All of the non CUDA C files are compiled using the standard +# build rules specified by CMAKE and the cuda files are compiled to object +# files using nvcc and the host compiler. In addition CUDA_INCLUDE_DIRS is +# added automatically to include_directories(). Some standard CMake target +# calls can be used on the target after calling this macro +# (e.g. set_target_properties and target_link_libraries), but setting +# properties that adjust compilation flags will not affect code compiled by +# nvcc. Such flags should be modified before calling CUDA_ADD_EXECUTABLE, +# CUDA_ADD_LIBRARY or CUDA_WRAP_SRCS. +# +# CUDA_ADD_LIBRARY( cuda_target file0 file1 ... +# [STATIC | SHARED | MODULE] [EXCLUDE_FROM_ALL] [OPTIONS ...] ) +# -- Same as CUDA_ADD_EXECUTABLE except that a library is created. +# +# CUDA_BUILD_CLEAN_TARGET() +# -- Creates a convience target that deletes all the dependency files +# generated. You should make clean after running this target to ensure the +# dependency files get regenerated. +# +# CUDA_COMPILE( generated_files file0 file1 ... [STATIC | SHARED | MODULE] +# [OPTIONS ...] ) +# -- Returns a list of generated files from the input source files to be used +# with ADD_LIBRARY or ADD_EXECUTABLE. +# +# CUDA_COMPILE_PTX( generated_files file0 file1 ... [OPTIONS ...] ) +# -- Returns a list of PTX files generated from the input source files. +# +# CUDA_INCLUDE_DIRECTORIES( path0 path1 ... ) +# -- Sets the directories that should be passed to nvcc +# (e.g. nvcc -Ipath0 -Ipath1 ... ). These paths usually contain other .cu +# files. +# +# CUDA_WRAP_SRCS ( cuda_target format generated_files file0 file1 ... +# [STATIC | SHARED | MODULE] [OPTIONS ...] ) +# -- This is where all the magic happens. CUDA_ADD_EXECUTABLE, +# CUDA_ADD_LIBRARY, CUDA_COMPILE, and CUDA_COMPILE_PTX all call this +# function under the hood. +# +# Given the list of files (file0 file1 ... fileN) this macro generates +# custom commands that generate either PTX or linkable objects (use "PTX" or +# "OBJ" for the format argument to switch). Files that don't end with .cu +# or have the HEADER_FILE_ONLY property are ignored. +# +# The arguments passed in after OPTIONS are extra command line options to +# give to nvcc. You can also specify per configuration options by +# specifying the name of the configuration followed by the options. General +# options must preceed configuration specific options. Not all +# configurations need to be specified, only the ones provided will be used. +# +# OPTIONS -DFLAG=2 "-DFLAG_OTHER=space in flag" +# DEBUG -g +# RELEASE --use_fast_math +# RELWITHDEBINFO --use_fast_math;-g +# MINSIZEREL --use_fast_math +# +# For certain configurations (namely VS generating object files with +# CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE set to ON), no generated file will +# be produced for the given cuda file. This is because when you add the +# cuda file to Visual Studio it knows that this file produces an object file +# and will link in the resulting object file automatically. +# +# This script will also generate a separate cmake script that is used at +# build time to invoke nvcc. This is for several reasons. +# +# 1. nvcc can return negative numbers as return values which confuses +# Visual Studio into thinking that the command succeeded. The script now +# checks the error codes and produces errors when there was a problem. +# +# 2. nvcc has been known to not delete incomplete results when it +# encounters problems. This confuses build systems into thinking the +# target was generated when in fact an unusable file exists. The script +# now deletes the output files if there was an error. +# +# 3. By putting all the options that affect the build into a file and then +# make the build rule dependent on the file, the output files will be +# regenerated when the options change. +# +# This script also looks at optional arguments STATIC, SHARED, or MODULE to +# determine when to target the object compilation for a shared library. +# BUILD_SHARED_LIBS is ignored in CUDA_WRAP_SRCS, but it is respected in +# CUDA_ADD_LIBRARY. On some systems special flags are added for building +# objects intended for shared libraries. A preprocessor macro, +# _EXPORTS is defined when a shared library compilation is +# detected. +# +# Flags passed into add_definitions with -D or /D are passed along to nvcc. +# +# The script defines the following variables: +# +# CUDA_VERSION_MAJOR -- The major version of cuda as reported by nvcc. +# CUDA_VERSION_MINOR -- The minor version. +# CUDA_VERSION +# CUDA_VERSION_STRING -- CUDA_VERSION_MAJOR.CUDA_VERSION_MINOR +# +# CUDA_TOOLKIT_ROOT_DIR -- Path to the CUDA Toolkit (defined if not set). +# CUDA_SDK_ROOT_DIR -- Path to the CUDA SDK. Use this to find files in the +# SDK. This script will not directly support finding +# specific libraries or headers, as that isn't +# supported by NVIDIA. If you want to change +# libraries when the path changes see the +# FindCUDA.cmake script for an example of how to clear +# these variables. There are also examples of how to +# use the CUDA_SDK_ROOT_DIR to locate headers or +# libraries, if you so choose (at your own risk). +# CUDA_INCLUDE_DIRS -- Include directory for cuda headers. Added automatically +# for CUDA_ADD_EXECUTABLE and CUDA_ADD_LIBRARY. +# CUDA_LIBRARIES -- Cuda RT library. +# CUDA_CUFFT_LIBRARIES -- Device or emulation library for the Cuda FFT +# implementation (alternative to: +# CUDA_ADD_CUFFT_TO_TARGET macro) +# CUDA_CUBLAS_LIBRARIES -- Device or emulation library for the Cuda BLAS +# implementation (alterative to: +# CUDA_ADD_CUBLAS_TO_TARGET macro). +# +# +# James Bigler, NVIDIA Corp (nvidia.com - jbigler) +# Abe Stephens, SCI Institute -- http://www.sci.utah.edu/~abe/FindCuda.html +# +# Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. +# +# Copyright (c) 2007-2009 +# Scientific Computing and Imaging Institute, University of Utah +# +# This code is licensed under the MIT License. See the FindCUDA.cmake script +# for the text of the license. + +# The MIT License +# +# License for the specific language governing rights and limitations under +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the "Software"), +# to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, +# and/or sell copies of the Software, and to permit persons to whom the +# Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. +# +############################################################################### + +# FindCUDA.cmake + +# We need to have at least this version to support the VERSION_LESS argument to 'if' (2.6.2) and unset (2.6.3) +cmake_policy(PUSH) +cmake_minimum_required(VERSION 2.6.3) +cmake_policy(POP) + +# This macro helps us find the location of helper files we will need the full path to +macro(CUDA_FIND_HELPER_FILE _name _extension) + set(_full_name "${_name}.${_extension}") + # CMAKE_CURRENT_LIST_FILE contains the full path to the file currently being + # processed. Using this variable, we can pull out the current path, and + # provide a way to get access to the other files we need local to here. + get_filename_component(CMAKE_CURRENT_LIST_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) + find_file(CUDA_${_name} ${_full_name} PATHS ${CMAKE_CURRENT_LIST_DIR}/FindCUDA NO_DEFAULT_PATH) + if(NOT CUDA_${_name}) + set(error_message "${_full_name} not found in CMAKE_MODULE_PATH") + if(CUDA_FIND_REQUIRED) + message(FATAL_ERROR "${error_message}") + else(CUDA_FIND_REQUIRED) + if(NOT CUDA_FIND_QUIETLY) + message(STATUS "${error_message}") + endif(NOT CUDA_FIND_QUIETLY) + endif(CUDA_FIND_REQUIRED) + endif(NOT CUDA_${_name}) + # Set this variable as internal, so the user isn't bugged with it. + set(CUDA_${_name} ${CUDA_${_name}} CACHE INTERNAL "Location of ${_full_name}" FORCE) +endmacro(CUDA_FIND_HELPER_FILE) + +##################################################################### +## CUDA_INCLUDE_NVCC_DEPENDENCIES +## + +# So we want to try and include the dependency file if it exists. If +# it doesn't exist then we need to create an empty one, so we can +# include it. + +# If it does exist, then we need to check to see if all the files it +# depends on exist. If they don't then we should clear the dependency +# file and regenerate it later. This covers the case where a header +# file has disappeared or moved. + +macro(CUDA_INCLUDE_NVCC_DEPENDENCIES dependency_file) + set(CUDA_NVCC_DEPEND) + set(CUDA_NVCC_DEPEND_REGENERATE FALSE) + + + # Include the dependency file. Create it first if it doesn't exist . The + # INCLUDE puts a dependency that will force CMake to rerun and bring in the + # new info when it changes. DO NOT REMOVE THIS (as I did and spent a few + # hours figuring out why it didn't work. + if(NOT EXISTS ${dependency_file}) + file(WRITE ${dependency_file} "#FindCUDA.cmake generated file. Do not edit.\n") + endif() + # Always include this file to force CMake to run again next + # invocation and rebuild the dependencies. + #message("including dependency_file = ${dependency_file}") + include(${dependency_file}) + + # Now we need to verify the existence of all the included files + # here. If they aren't there we need to just blank this variable and + # make the file regenerate again. +# if(DEFINED CUDA_NVCC_DEPEND) +# message("CUDA_NVCC_DEPEND set") +# else() +# message("CUDA_NVCC_DEPEND NOT set") +# endif() + if(CUDA_NVCC_DEPEND) + #message("CUDA_NVCC_DEPEND true") + foreach(f ${CUDA_NVCC_DEPEND}) + #message("searching for ${f}") + if(NOT EXISTS ${f}) + #message("file ${f} not found") + set(CUDA_NVCC_DEPEND_REGENERATE TRUE) + endif() + endforeach(f) + else(CUDA_NVCC_DEPEND) + #message("CUDA_NVCC_DEPEND false") + # No dependencies, so regenerate the file. + set(CUDA_NVCC_DEPEND_REGENERATE TRUE) + endif(CUDA_NVCC_DEPEND) + + #message("CUDA_NVCC_DEPEND_REGENERATE = ${CUDA_NVCC_DEPEND_REGENERATE}") + # No incoming dependencies, so we need to generate them. Make the + # output depend on the dependency file itself, which should cause the + # rule to re-run. + if(CUDA_NVCC_DEPEND_REGENERATE) + file(WRITE ${dependency_file} "#FindCUDA.cmake generated file. Do not edit.\n") + endif(CUDA_NVCC_DEPEND_REGENERATE) + +endmacro(CUDA_INCLUDE_NVCC_DEPENDENCIES) + +############################################################################### +############################################################################### +# Setup variables' defaults +############################################################################### +############################################################################### + +# Allow the user to specify if the device code is supposed to be 32 or 64 bit. +if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(CUDA_64_BIT_DEVICE_CODE_DEFAULT ON) +else() + set(CUDA_64_BIT_DEVICE_CODE_DEFAULT OFF) +endif() +option(CUDA_64_BIT_DEVICE_CODE "Compile device code in 64 bit mode" ${CUDA_64_BIT_DEVICE_CODE_DEFAULT}) + +# Attach the build rule to the source file in VS. This option +option(CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE "Attach the build rule to the CUDA source file. Enable only when the CUDA source file is added to at most one target." ON) + +# Prints out extra information about the cuda file during compilation +option(CUDA_BUILD_CUBIN "Generate and parse .cubin files in Device mode." OFF) + +# Set whether we are using emulation or device mode. +option(CUDA_BUILD_EMULATION "Build in Emulation mode" OFF) + +# Where to put the generated output. +set(CUDA_GENERATED_OUTPUT_DIR "" CACHE PATH "Directory to put all the output files. If blank it will default to the CMAKE_CURRENT_BINARY_DIR") + +# Parse HOST_COMPILATION mode. +option(CUDA_HOST_COMPILATION_CPP "Generated file extension" ON) + +# Extra user settable flags +set(CUDA_NVCC_FLAGS "" CACHE STRING "Semi-colon delimit multiple arguments.") + +# Propagate the host flags to the host compiler via -Xcompiler +option(CUDA_PROPAGATE_HOST_FLAGS "Propage C/CXX_FLAGS and friends to the host compiler via -Xcompile" ON) + +# Specifies whether the commands used when compiling the .cu file will be printed out. +option(CUDA_VERBOSE_BUILD "Print out the commands run while compiling the CUDA source file. With the Makefile generator this defaults to VERBOSE variable specified on the command line, but can be forced on with this option." OFF) + +mark_as_advanced( + CUDA_64_BIT_DEVICE_CODE + CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE + CUDA_GENERATED_OUTPUT_DIR + CUDA_HOST_COMPILATION_CPP + CUDA_NVCC_FLAGS + CUDA_PROPAGATE_HOST_FLAGS + ) + +# Makefile and similar generators don't define CMAKE_CONFIGURATION_TYPES, so we +# need to add another entry for the CMAKE_BUILD_TYPE. We also need to add the +# standerd set of 4 build types (Debug, MinSizeRel, Release, and RelWithDebInfo) +# for completeness. We need run this loop in order to accomodate the addition +# of extra configuration types. Duplicate entries will be removed by +# REMOVE_DUPLICATES. +set(CUDA_configuration_types ${CMAKE_CONFIGURATION_TYPES} ${CMAKE_BUILD_TYPE} Debug MinSizeRel Release RelWithDebInfo) +list(REMOVE_DUPLICATES CUDA_configuration_types) +foreach(config ${CUDA_configuration_types}) + string(TOUPPER ${config} config_upper) + set(CUDA_NVCC_FLAGS_${config_upper} "" CACHE STRING "Semi-colon delimit multiple arguments.") + mark_as_advanced(CUDA_NVCC_FLAGS_${config_upper}) +endforeach() + +############################################################################### +############################################################################### +# Locate CUDA, Set Build Type, etc. +############################################################################### +############################################################################### + +# Check to see if the CUDA_TOOLKIT_ROOT_DIR and CUDA_SDK_ROOT_DIR have changed, +# if they have then clear the cache variables, so that will be detected again. +if(NOT "${CUDA_TOOLKIT_ROOT_DIR}" STREQUAL "${CUDA_TOOLKIT_ROOT_DIR_INTERNAL}") + unset(CUDA_NVCC_EXECUTABLE CACHE) + unset(CUDA_VERSION CACHE) + unset(CUDA_TOOLKIT_INCLUDE CACHE) + unset(CUDA_CUDART_LIBRARY CACHE) + if(CUDA_VERSION VERSION_EQUAL "3.0") + # This only existed in the 3.0 version of the CUDA toolkit + unset(CUDA_CUDARTEMU_LIBRARY CACHE) + endif() + unset(CUDA_CUDA_LIBRARY CACHE) + unset(CUDA_cublas_LIBRARY CACHE) + unset(CUDA_cublasemu_LIBRARY CACHE) + unset(CUDA_cufft_LIBRARY CACHE) + unset(CUDA_cufftemu_LIBRARY CACHE) +endif() + +if(NOT "${CUDA_SDK_ROOT_DIR}" STREQUAL "${CUDA_SDK_ROOT_DIR_INTERNAL}") + # No specific variables to catch. Use this kind of code before calling + # find_package(CUDA) to clean up any variables that may depend on this path. + + # unset(MY_SPECIAL_CUDA_SDK_INCLUDE_DIR CACHE) + # unset(MY_SPECIAL_CUDA_SDK_LIBRARY CACHE) +endif() + +# Search for the cuda distribution. +if(NOT CUDA_TOOLKIT_ROOT_DIR) + + # Search in the CUDA_BIN_PATH first. + find_path(CUDA_TOOLKIT_ROOT_DIR + NAMES nvcc nvcc.exe + PATHS ENV CUDA_BIN_PATH + DOC "Toolkit location." + NO_DEFAULT_PATH + ) + # Now search default paths + find_path(CUDA_TOOLKIT_ROOT_DIR + NAMES nvcc nvcc.exe + PATHS /usr/local/bin + /usr/local/cuda/bin + DOC "Toolkit location." + ) + + if (CUDA_TOOLKIT_ROOT_DIR) + string(REGEX REPLACE "[/\\\\]?bin[64]*[/\\\\]?$" "" CUDA_TOOLKIT_ROOT_DIR ${CUDA_TOOLKIT_ROOT_DIR}) + # We need to force this back into the cache. + set(CUDA_TOOLKIT_ROOT_DIR ${CUDA_TOOLKIT_ROOT_DIR} CACHE PATH "Toolkit location." FORCE) + endif(CUDA_TOOLKIT_ROOT_DIR) + if (NOT EXISTS ${CUDA_TOOLKIT_ROOT_DIR}) + if(CUDA_FIND_REQUIRED) + message(FATAL_ERROR "Specify CUDA_TOOLKIT_ROOT_DIR") + elseif(NOT CUDA_FIND_QUIETLY) + message("CUDA_TOOLKIT_ROOT_DIR not found or specified") + endif() + endif (NOT EXISTS ${CUDA_TOOLKIT_ROOT_DIR}) +endif (NOT CUDA_TOOLKIT_ROOT_DIR) + +# CUDA_NVCC_EXECUTABLE +find_program(CUDA_NVCC_EXECUTABLE + NAMES nvcc + PATHS "${CUDA_TOOLKIT_ROOT_DIR}/bin" + "${CUDA_TOOLKIT_ROOT_DIR}/bin64" + ENV CUDA_BIN_PATH + NO_DEFAULT_PATH + ) +# Search default search paths, after we search our own set of paths. +find_program(CUDA_NVCC_EXECUTABLE nvcc) +mark_as_advanced(CUDA_NVCC_EXECUTABLE) + +if(CUDA_NVCC_EXECUTABLE AND NOT CUDA_VERSION) + # Compute the version. + execute_process (COMMAND ${CUDA_NVCC_EXECUTABLE} "--version" OUTPUT_VARIABLE NVCC_OUT) + string(REGEX REPLACE ".*release ([0-9]+)\\.([0-9]+).*" "\\1" CUDA_VERSION_MAJOR ${NVCC_OUT}) + string(REGEX REPLACE ".*release ([0-9]+)\\.([0-9]+).*" "\\2" CUDA_VERSION_MINOR ${NVCC_OUT}) + set(CUDA_VERSION "${CUDA_VERSION_MAJOR}.${CUDA_VERSION_MINOR}" CACHE STRING "Version of CUDA as computed from nvcc.") + mark_as_advanced(CUDA_VERSION) +else() + # Need to set these based off of the cached value + string(REGEX REPLACE "([0-9]+)\\.([0-9]+).*" "\\1" CUDA_VERSION_MAJOR "${CUDA_VERSION}") + string(REGEX REPLACE "([0-9]+)\\.([0-9]+).*" "\\2" CUDA_VERSION_MINOR "${CUDA_VERSION}") +endif() + +# Always set this convenience variable +set(CUDA_VERSION_STRING "${CUDA_VERSION}") + +# Here we need to determine if the version we found is acceptable. We will +# assume that is unless CUDA_FIND_VERSION_EXACT or CUDA_FIND_VERSION is +# specified. The presence of either of these options checks the version +# string and signals if the version is acceptable or not. +set(_cuda_version_acceptable TRUE) +# +if(CUDA_FIND_VERSION_EXACT AND NOT CUDA_VERSION VERSION_EQUAL CUDA_FIND_VERSION) + set(_cuda_version_acceptable FALSE) +endif() +# +if(CUDA_FIND_VERSION AND CUDA_VERSION VERSION_LESS CUDA_FIND_VERSION) + set(_cuda_version_acceptable FALSE) +endif() +# +if(NOT _cuda_version_acceptable) + set(_cuda_error_message "Requested CUDA version ${CUDA_FIND_VERSION}, but found unacceptable version ${CUDA_VERSION}") + if(CUDA_FIND_REQUIRED) + message("${_cuda_error_message}") + elseif(NOT CUDA_FIND_QUIETLY) + message("${_cuda_error_message}") + endif() +endif() + +# CUDA_TOOLKIT_INCLUDE +find_path(CUDA_TOOLKIT_INCLUDE + device_functions.h # Header included in toolkit + PATHS "${CUDA_TOOLKIT_ROOT_DIR}/include" + ENV CUDA_INC_PATH + NO_DEFAULT_PATH + ) +# Search default search paths, after we search our own set of paths. +find_path(CUDA_TOOLKIT_INCLUDE device_functions.h) +mark_as_advanced(CUDA_TOOLKIT_INCLUDE) + +# Set the user list of include dir to nothing to initialize it. +set (CUDA_NVCC_INCLUDE_ARGS_USER "") +set (CUDA_INCLUDE_DIRS ${CUDA_TOOLKIT_INCLUDE}) + +macro(FIND_LIBRARY_LOCAL_FIRST _var _names _doc) + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + # CUDA 3.2+ on Windows moved the library directoryies, so we need the new + # and old paths. + set(_cuda_64bit_lib_dir + "${CUDA_TOOLKIT_ROOT_DIR}/lib/x64" + "${CUDA_TOOLKIT_ROOT_DIR}/lib64" + ) + endif() + # CUDA 3.2+ on Windows moved the library directories, so we need to new + # (lib/Win32) and the old path (lib). + find_library(${_var} + NAMES ${_names} + PATHS ${_cuda_64bit_lib_dir} + "${CUDA_TOOLKIT_ROOT_DIR}/lib/Win32" + "${CUDA_TOOLKIT_ROOT_DIR}/lib" + ENV CUDA_LIB_PATH + DOC ${_doc} + NO_DEFAULT_PATH + ) + # Search default search paths, after we search our own set of paths. + find_library(${_var} NAMES ${_names} DOC ${_doc}) +endmacro() + +# CUDA_LIBRARIES +find_library_local_first(CUDA_CUDART_LIBRARY cudart "\"cudart\" library") +if(CUDA_VERSION VERSION_EQUAL "3.0") + # The cudartemu library only existed for the 3.0 version of CUDA. + find_library_local_first(CUDA_CUDARTEMU_LIBRARY cudartemu "\"cudartemu\" library") + mark_as_advanced( + CUDA_CUDARTEMU_LIBRARY + ) +endif() +# If we are using emulation mode and we found the cudartemu library then use +# that one instead of cudart. +if(CUDA_BUILD_EMULATION AND CUDA_CUDARTEMU_LIBRARY) + set(CUDA_LIBRARIES ${CUDA_CUDARTEMU_LIBRARY}) +else() +set(CUDA_LIBRARIES ${CUDA_CUDART_LIBRARY}) +endif() +if(APPLE) + # We need to add the path to cudart to the linker using rpath, since the + # library name for the cuda libraries is prepended with @rpath. + if(CUDA_BUILD_EMULATION AND CUDA_CUDARTEMU_LIBRARY) + get_filename_component(_cuda_path_to_cudart "${CUDA_CUDARTEMU_LIBRARY}" PATH) + else() + get_filename_component(_cuda_path_to_cudart "${CUDA_CUDART_LIBRARY}" PATH) + endif() + if(_cuda_path_to_cudart) + list(APPEND CUDA_LIBRARIES -Wl,-rpath "-Wl,${_cuda_path_to_cudart}") + endif() +endif() + +# 1.1 toolkit on linux doesn't appear to have a separate library on +# some platforms. +find_library_local_first(CUDA_CUDA_LIBRARY cuda "\"cuda\" library (older versions only).") + +# Add cuda library to the link line only if it is found. +if (CUDA_CUDA_LIBRARY) + set(CUDA_LIBRARIES ${CUDA_LIBRARIES} ${CUDA_CUDA_LIBRARY}) +endif(CUDA_CUDA_LIBRARY) + +mark_as_advanced( + CUDA_CUDA_LIBRARY + CUDA_CUDART_LIBRARY + ) + +####################### +# Look for some of the toolkit helper libraries +macro(FIND_CUDA_HELPER_LIBS _name) + find_library_local_first(CUDA_${_name}_LIBRARY ${_name} "\"${_name}\" library") + mark_as_advanced(CUDA_${_name}_LIBRARY) +endmacro(FIND_CUDA_HELPER_LIBS) + +####################### +# Disable emulation for v3.1 onward +if(CUDA_VERSION VERSION_GREATER "3.0") + if(CUDA_BUILD_EMULATION) + message(FATAL_ERROR "CUDA_BUILD_EMULATION is not supported in version 3.1 and onwards. You must disable it to proceed. You have version ${CUDA_VERSION}.") + endif() +endif() + +# Search for cufft and cublas libraries. +if(CUDA_VERSION VERSION_LESS "3.1") + # Emulation libraries aren't available in version 3.1 onward. +find_cuda_helper_libs(cufftemu) +find_cuda_helper_libs(cublasemu) +endif() +find_cuda_helper_libs(cufft) +find_cuda_helper_libs(cublas) + +if (CUDA_BUILD_EMULATION) + set(CUDA_CUFFT_LIBRARIES ${CUDA_cufftemu_LIBRARY}) + set(CUDA_CUBLAS_LIBRARIES ${CUDA_cublasemu_LIBRARY}) +else() + set(CUDA_CUFFT_LIBRARIES ${CUDA_cufft_LIBRARY}) + set(CUDA_CUBLAS_LIBRARIES ${CUDA_cublas_LIBRARY}) +endif() + +######################## +# Look for the SDK stuff. As of CUDA 3.0 NVSDKCUDA_ROOT has been replaced with +# NVSDKCOMPUTE_ROOT with the old CUDA C contents moved into the C subdirectory +find_path(CUDA_SDK_ROOT_DIR common/inc/cutil.h + "$ENV{HOME}/NVIDIA_GPU_Computing_SDK/C" + "$ENV{NVSDKCOMPUTE_ROOT}/C" + "$ENV{NVSDKCUDA_ROOT}" + "[HKEY_LOCAL_MACHINE\\SOFTWARE\\NVIDIA Corporation\\Installed Products\\NVIDIA SDK 10\\Compute;InstallDir]" + "/Developer/GPU\ Computing/C" + ) + +# Keep the CUDA_SDK_ROOT_DIR first in order to be able to override the +# environment variables. +set(CUDA_SDK_SEARCH_PATH + "${CUDA_SDK_ROOT_DIR}" + "${CUDA_TOOLKIT_ROOT_DIR}/local/NVSDK0.2" + "${CUDA_TOOLKIT_ROOT_DIR}/NVSDK0.2" + "${CUDA_TOOLKIT_ROOT_DIR}/NV_CUDA_SDK" + "$ENV{HOME}/NVIDIA_CUDA_SDK" + "$ENV{HOME}/NVIDIA_CUDA_SDK_MACOSX" + "$ENV{HOME}/NVIDIA_GPU_Computing_SDK/C" + "/Developer/CUDA" + ) + +# Example of how to find an include file from the CUDA_SDK_ROOT_DIR + +# find_path(CUDA_CUT_INCLUDE_DIR +# cutil.h +# PATHS ${CUDA_SDK_SEARCH_PATH} +# PATH_SUFFIXES "common/inc" +# DOC "Location of cutil.h" +# NO_DEFAULT_PATH +# ) +# # Now search system paths +# find_path(CUDA_CUT_INCLUDE_DIR cutil.h DOC "Location of cutil.h") + +# mark_as_advanced(CUDA_CUT_INCLUDE_DIR) + + +# Example of how to find a library in the CUDA_SDK_ROOT_DIR + +# # cutil library is called cutil64 for 64 bit builds on windows. We don't want +# # to get these confused, so we are setting the name based on the word size of +# # the build. + +# if(CMAKE_SIZEOF_VOID_P EQUAL 8) +# set(cuda_cutil_name cutil64) +# else(CMAKE_SIZEOF_VOID_P EQUAL 8) +# set(cuda_cutil_name cutil32) +# endif(CMAKE_SIZEOF_VOID_P EQUAL 8) + +# find_library(CUDA_CUT_LIBRARY +# NAMES cutil ${cuda_cutil_name} +# PATHS ${CUDA_SDK_SEARCH_PATH} +# # The new version of the sdk shows up in common/lib, but the old one is in lib +# PATH_SUFFIXES "common/lib" "lib" +# DOC "Location of cutil library" +# NO_DEFAULT_PATH +# ) +# # Now search system paths +# find_library(CUDA_CUT_LIBRARY NAMES cutil ${cuda_cutil_name} DOC "Location of cutil library") +# mark_as_advanced(CUDA_CUT_LIBRARY) +# set(CUDA_CUT_LIBRARIES ${CUDA_CUT_LIBRARY}) + + + +############################# +# Check for required components +set(CUDA_FOUND TRUE) + +set(CUDA_TOOLKIT_ROOT_DIR_INTERNAL "${CUDA_TOOLKIT_ROOT_DIR}" CACHE INTERNAL + "This is the value of the last time CUDA_TOOLKIT_ROOT_DIR was set successfully." FORCE) +set(CUDA_SDK_ROOT_DIR_INTERNAL "${CUDA_SDK_ROOT_DIR}" CACHE INTERNAL + "This is the value of the last time CUDA_SDK_ROOT_DIR was set successfully." FORCE) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(CUDA DEFAULT_MSG + CUDA_TOOLKIT_ROOT_DIR + CUDA_NVCC_EXECUTABLE + CUDA_INCLUDE_DIRS + CUDA_CUDART_LIBRARY + _cuda_version_acceptable + ) + + + +############################################################################### +############################################################################### +# Macros +############################################################################### +############################################################################### + +############################################################################### +# Add include directories to pass to the nvcc command. +macro(CUDA_INCLUDE_DIRECTORIES) + foreach(dir ${ARGN}) + list(APPEND CUDA_NVCC_INCLUDE_ARGS_USER "-I${dir}") + endforeach(dir ${ARGN}) +endmacro(CUDA_INCLUDE_DIRECTORIES) + + +############################################################################## +cuda_find_helper_file(parse_cubin cmake) +cuda_find_helper_file(make2cmake cmake) +cuda_find_helper_file(run_nvcc cmake) + +############################################################################## +# Separate the OPTIONS out from the sources +# +macro(CUDA_GET_SOURCES_AND_OPTIONS _sources _cmake_options _options) + set( ${_sources} ) + set( ${_cmake_options} ) + set( ${_options} ) + set( _found_options FALSE ) + foreach(arg ${ARGN}) + if(arg STREQUAL "OPTIONS") + set( _found_options TRUE ) + elseif( + arg STREQUAL "WIN32" OR + arg STREQUAL "MACOSX_BUNDLE" OR + arg STREQUAL "EXCLUDE_FROM_ALL" OR + arg STREQUAL "STATIC" OR + arg STREQUAL "SHARED" OR + arg STREQUAL "MODULE" + ) + list(APPEND ${_cmake_options} "${arg}") + else() + if ( _found_options ) + list(APPEND ${_options} "${arg}") + else() + # Assume this is a file + list(APPEND ${_sources} "${arg}") + endif() + endif() + endforeach() +endmacro() + +############################################################################## +# Parse the OPTIONS from ARGN and set the variables prefixed by _option_prefix +# +macro(CUDA_PARSE_NVCC_OPTIONS _option_prefix) + set( _found_config ) + foreach(arg ${ARGN}) + # Determine if we are dealing with a perconfiguration flag + foreach(config ${CUDA_configuration_types}) + string(TOUPPER ${config} config_upper) + if (arg STREQUAL "${config_upper}") + set( _found_config _${arg}) + # Set arg to nothing to keep it from being processed further + set( arg ) + endif() + endforeach() + + if ( arg ) + list(APPEND ${_option_prefix}${_found_config} "${arg}") + endif() + endforeach() +endmacro() + +############################################################################## +# Helper to add the include directory for CUDA only once +function(CUDA_ADD_CUDA_INCLUDE_ONCE) + get_directory_property(_include_directories INCLUDE_DIRECTORIES) + set(_add TRUE) + if(_include_directories) + foreach(dir ${_include_directories}) + if("${dir}" STREQUAL "${CUDA_INCLUDE_DIRS}") + set(_add FALSE) + endif() + endforeach() + endif() + if(_add) + include_directories(${CUDA_INCLUDE_DIRS}) + endif() +endfunction() + +function(CUDA_BUILD_SHARED_LIBRARY shared_flag) + set(cmake_args ${ARGN}) + # If SHARED, MODULE, or STATIC aren't already in the list of arguments, then + # add SHARED or STATIC based on the value of BUILD_SHARED_LIBS. + list(FIND cmake_args SHARED _cuda_found_SHARED) + list(FIND cmake_args MODULE _cuda_found_MODULE) + list(FIND cmake_args STATIC _cuda_found_STATIC) + if( _cuda_found_SHARED GREATER -1 OR + _cuda_found_MODULE GREATER -1 OR + _cuda_found_STATIC GREATER -1) + set(_cuda_build_shared_libs) + else() + if (BUILD_SHARED_LIBS) + set(_cuda_build_shared_libs SHARED) + else() + set(_cuda_build_shared_libs STATIC) + endif() + endif() + set(${shared_flag} ${_cuda_build_shared_libs} PARENT_SCOPE) +endfunction() + +############################################################################## +# This helper macro populates the following variables and setups up custom +# commands and targets to invoke the nvcc compiler to generate C or PTX source +# dependent upon the format parameter. The compiler is invoked once with -M +# to generate a dependency file and a second time with -cuda or -ptx to generate +# a .cpp or .ptx file. +# INPUT: +# cuda_target - Target name +# format - PTX or OBJ +# FILE1 .. FILEN - The remaining arguments are the sources to be wrapped. +# OPTIONS - Extra options to NVCC +# OUTPUT: +# generated_files - List of generated files +############################################################################## +############################################################################## + +macro(CUDA_WRAP_SRCS cuda_target format generated_files) + + if( ${format} MATCHES "PTX" ) + set( compile_to_ptx ON ) + elseif( ${format} MATCHES "OBJ") + set( compile_to_ptx OFF ) + else() + message( FATAL_ERROR "Invalid format flag passed to CUDA_WRAP_SRCS: '${format}'. Use OBJ or PTX.") + endif() + + # Set up all the command line flags here, so that they can be overriden on a per target basis. + + set(nvcc_flags "") + + # Emulation if the card isn't present. + if (CUDA_BUILD_EMULATION) + # Emulation. + set(nvcc_flags ${nvcc_flags} --device-emulation -D_DEVICEEMU -g) + else(CUDA_BUILD_EMULATION) + # Device mode. No flags necessary. + endif(CUDA_BUILD_EMULATION) + + if(CUDA_HOST_COMPILATION_CPP) + set(CUDA_C_OR_CXX CXX) + else(CUDA_HOST_COMPILATION_CPP) + if(CUDA_VERSION VERSION_LESS "3.0") + set(nvcc_flags ${nvcc_flags} --host-compilation C) + else() + message(WARNING "--host-compilation flag is deprecated in CUDA version >= 3.0. Removing --host-compilation C flag" ) + endif() + set(CUDA_C_OR_CXX C) + endif(CUDA_HOST_COMPILATION_CPP) + + set(generated_extension ${CMAKE_${CUDA_C_OR_CXX}_OUTPUT_EXTENSION}) + + if(CUDA_64_BIT_DEVICE_CODE) + set(nvcc_flags ${nvcc_flags} -m64) + else() + set(nvcc_flags ${nvcc_flags} -m32) + endif() + + # This needs to be passed in at this stage, because VS needs to fill out the + # value of VCInstallDir from within VS. + if(CMAKE_GENERATOR MATCHES "Visual Studio") + if( CMAKE_SIZEOF_VOID_P EQUAL 8 ) + # Add nvcc flag for 64b Windows + set(ccbin_flags -D "\"CCBIN:PATH=$(VCInstallDir)bin\"" ) + endif() + endif() + + # Figure out which configure we will use and pass that in as an argument to + # the script. We need to defer the decision until compilation time, because + # for VS projects we won't know if we are making a debug or release build + # until build time. + if(CMAKE_GENERATOR MATCHES "Visual Studio") + set( CUDA_build_configuration "$(ConfigurationName)" ) + else() + set( CUDA_build_configuration "${CMAKE_BUILD_TYPE}") + endif() + + # Initialize our list of includes with the user ones followed by the CUDA system ones. + set(CUDA_NVCC_INCLUDE_ARGS ${CUDA_NVCC_INCLUDE_ARGS_USER} "-I${CUDA_INCLUDE_DIRS}") + # Get the include directories for this directory and use them for our nvcc command. + get_directory_property(CUDA_NVCC_INCLUDE_DIRECTORIES INCLUDE_DIRECTORIES) + if(CUDA_NVCC_INCLUDE_DIRECTORIES) + foreach(dir ${CUDA_NVCC_INCLUDE_DIRECTORIES}) + list(APPEND CUDA_NVCC_INCLUDE_ARGS "-I${dir}") + endforeach() + endif() + + # Reset these variables + set(CUDA_WRAP_OPTION_NVCC_FLAGS) + foreach(config ${CUDA_configuration_types}) + string(TOUPPER ${config} config_upper) + set(CUDA_WRAP_OPTION_NVCC_FLAGS_${config_upper}) + endforeach() + + CUDA_GET_SOURCES_AND_OPTIONS(_cuda_wrap_sources _cuda_wrap_cmake_options _cuda_wrap_options ${ARGN}) + CUDA_PARSE_NVCC_OPTIONS(CUDA_WRAP_OPTION_NVCC_FLAGS ${_cuda_wrap_options}) + + # Figure out if we are building a shared library. BUILD_SHARED_LIBS is + # respected in CUDA_ADD_LIBRARY. + set(_cuda_build_shared_libs FALSE) + # SHARED, MODULE + list(FIND _cuda_wrap_cmake_options SHARED _cuda_found_SHARED) + list(FIND _cuda_wrap_cmake_options MODULE _cuda_found_MODULE) + if(_cuda_found_SHARED GREATER -1 OR _cuda_found_MODULE GREATER -1) + set(_cuda_build_shared_libs TRUE) + endif() + # STATIC + list(FIND _cuda_wrap_cmake_options STATIC _cuda_found_STATIC) + if(_cuda_found_STATIC GREATER -1) + set(_cuda_build_shared_libs FALSE) + endif() + + # CUDA_HOST_FLAGS + if(_cuda_build_shared_libs) + # If we are setting up code for a shared library, then we need to add extra flags for + # compiling objects for shared libraries. + set(CUDA_HOST_SHARED_FLAGS ${CMAKE_SHARED_LIBRARY_${CUDA_C_OR_CXX}_FLAGS}) + else() + set(CUDA_HOST_SHARED_FLAGS) + endif() + # Only add the CMAKE_{C,CXX}_FLAGS if we are propagating host flags. We + # always need to set the SHARED_FLAGS, though. + if(CUDA_PROPAGATE_HOST_FLAGS) + set(CUDA_HOST_FLAGS "set(CMAKE_HOST_FLAGS ${CMAKE_${CUDA_C_OR_CXX}_FLAGS} ${CUDA_HOST_SHARED_FLAGS})") + else() + set(CUDA_HOST_FLAGS "set(CMAKE_HOST_FLAGS ${CUDA_HOST_SHARED_FLAGS})") + endif() + + set(CUDA_NVCC_FLAGS_CONFIG "# Build specific configuration flags") + # Loop over all the configuration types to generate appropriate flags for run_nvcc.cmake + foreach(config ${CUDA_configuration_types}) + string(TOUPPER ${config} config_upper) + # CMAKE_FLAGS are strings and not lists. By not putting quotes around CMAKE_FLAGS + # we convert the strings to lists (like we want). + + if(CUDA_PROPAGATE_HOST_FLAGS) + # nvcc chokes on -g3 in versions previous to 3.0, so replace it with -g + if(CMAKE_COMPILER_IS_GNUCC AND CUDA_VERSION VERSION_LESS "3.0") + string(REPLACE "-g3" "-g" _cuda_C_FLAGS "${CMAKE_${CUDA_C_OR_CXX}_FLAGS_${config_upper}}") + else() + set(_cuda_C_FLAGS "${CMAKE_${CUDA_C_OR_CXX}_FLAGS_${config_upper}}") + endif() + + set(CUDA_HOST_FLAGS "${CUDA_HOST_FLAGS}\nset(CMAKE_HOST_FLAGS_${config_upper} ${_cuda_C_FLAGS})") + endif() + + # Note that if we ever want CUDA_NVCC_FLAGS_ to be string (instead of a list + # like it is currently), we can remove the quotes around the + # ${CUDA_NVCC_FLAGS_${config_upper}} variable like the CMAKE_HOST_FLAGS_ variable. + set(CUDA_NVCC_FLAGS_CONFIG "${CUDA_NVCC_FLAGS_CONFIG}\nset(CUDA_NVCC_FLAGS_${config_upper} \"${CUDA_NVCC_FLAGS_${config_upper}};;${CUDA_WRAP_OPTION_NVCC_FLAGS_${config_upper}}\")") + endforeach() + + if(compile_to_ptx) + # Don't use any of the host compilation flags for PTX targets. + set(CUDA_HOST_FLAGS) + set(CUDA_NVCC_FLAGS_CONFIG) + endif() + + # Get the list of definitions from the directory property + get_directory_property(CUDA_NVCC_DEFINITIONS COMPILE_DEFINITIONS) + if(CUDA_NVCC_DEFINITIONS) + foreach(_definition ${CUDA_NVCC_DEFINITIONS}) + list(APPEND nvcc_flags "-D${_definition}") + endforeach() + endif() + + if(_cuda_build_shared_libs) + list(APPEND nvcc_flags "-D${cuda_target}_EXPORTS") + endif() + + # Determine output directory + if(CUDA_GENERATED_OUTPUT_DIR) + set(cuda_compile_output_dir "${CUDA_GENERATED_OUTPUT_DIR}") + else() + set(cuda_compile_output_dir "${CMAKE_CURRENT_BINARY_DIR}") + endif() + + # Reset the output variable + set(_cuda_wrap_generated_files "") + + # Iterate over the macro arguments and create custom + # commands for all the .cu files. + foreach(file ${ARGN}) + # Ignore any file marked as a HEADER_FILE_ONLY + get_source_file_property(_is_header ${file} HEADER_FILE_ONLY) + if(${file} MATCHES ".*\\.cu$" AND NOT _is_header) + + # Add a custom target to generate a c or ptx file. ###################### + + get_filename_component( basename ${file} NAME ) + if( compile_to_ptx ) + set(generated_file_path "${cuda_compile_output_dir}") + set(generated_file_basename "${cuda_target}_generated_${basename}.ptx") + set(format_flag "-ptx") + file(MAKE_DIRECTORY "${cuda_compile_output_dir}") + else( compile_to_ptx ) + set(generated_file_path "${cuda_compile_output_dir}/${CMAKE_CFG_INTDIR}") + set(generated_file_basename "${cuda_target}_generated_${basename}${generated_extension}") + set(format_flag "-c") + endif( compile_to_ptx ) + + # Set all of our file names. Make sure that whatever filenames that have + # generated_file_path in them get passed in through as a command line + # argument, so that the ${CMAKE_CFG_INTDIR} gets expanded at run time + # instead of configure time. + set(generated_file "${generated_file_path}/${generated_file_basename}") + set(cmake_dependency_file "${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/${generated_file_basename}.depend") + set(NVCC_generated_dependency_file "${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/${generated_file_basename}.NVCC-depend") + set(generated_cubin_file "${generated_file_path}/${generated_file_basename}.cubin.txt") + set(custom_target_script "${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/${generated_file_basename}.cmake") + + # Setup properties for obj files: + if( NOT compile_to_ptx ) + set_source_files_properties("${generated_file}" + PROPERTIES + EXTERNAL_OBJECT true # This is an object file not to be compiled, but only be linked. + ) + endif() + + # Don't add CMAKE_CURRENT_SOURCE_DIR if the path is already an absolute path. + get_filename_component(file_path "${file}" PATH) + if(IS_ABSOLUTE "${file_path}") + set(source_file "${file}") + else() + set(source_file "${CMAKE_CURRENT_SOURCE_DIR}/${file}") + endif() + + # Bring in the dependencies. Creates a variable CUDA_NVCC_DEPEND ####### + cuda_include_nvcc_dependencies(${cmake_dependency_file}) + + # Convience string for output ########################################### + if(CUDA_BUILD_EMULATION) + set(cuda_build_type "Emulation") + else(CUDA_BUILD_EMULATION) + set(cuda_build_type "Device") + endif(CUDA_BUILD_EMULATION) + + # Build the NVCC made dependency file ################################### + set(build_cubin OFF) + if ( NOT CUDA_BUILD_EMULATION AND CUDA_BUILD_CUBIN ) + if ( NOT compile_to_ptx ) + set ( build_cubin ON ) + endif( NOT compile_to_ptx ) + endif( NOT CUDA_BUILD_EMULATION AND CUDA_BUILD_CUBIN ) + + # Configure the build script + configure_file("${CUDA_run_nvcc}" "${custom_target_script}" @ONLY) + + # So if a user specifies the same cuda file as input more than once, you + # can have bad things happen with dependencies. Here we check an option + # to see if this is the behavior they want. + if(CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE) + set(main_dep MAIN_DEPENDENCY ${source_file}) + else() + set(main_dep DEPENDS ${source_file}) + endif() + + if(CUDA_VERBOSE_BUILD) + set(verbose_output ON) + elseif(CMAKE_GENERATOR MATCHES "Makefiles") + set(verbose_output "$(VERBOSE)") + else() + set(verbose_output OFF) + endif() + + # Create up the comment string + file(RELATIVE_PATH generated_file_relative_path "${CMAKE_BINARY_DIR}" "${generated_file}") + if(compile_to_ptx) + set(cuda_build_comment_string "Building NVCC ptx file ${generated_file_relative_path}") + else() + set(cuda_build_comment_string "Building NVCC (${cuda_build_type}) object ${generated_file_relative_path}") + endif() + + # Build the generated file and dependency file ########################## + add_custom_command( + OUTPUT ${generated_file} + # These output files depend on the source_file and the contents of cmake_dependency_file + ${main_dep} + DEPENDS ${CUDA_NVCC_DEPEND} + DEPENDS ${custom_target_script} + # Make sure the output directory exists before trying to write to it. + COMMAND ${CMAKE_COMMAND} -E make_directory "${generated_file_path}" + COMMAND ${CMAKE_COMMAND} ARGS + -D verbose:BOOL=${verbose_output} + ${ccbin_flags} + -D build_configuration:STRING=${CUDA_build_configuration} + -D "generated_file:STRING=${generated_file}" + -D "generated_cubin_file:STRING=${generated_cubin_file}" + -P "${custom_target_script}" + COMMENT "${cuda_build_comment_string}" + ) + + # Make sure the build system knows the file is generated. + set_source_files_properties(${generated_file} PROPERTIES GENERATED TRUE) + + # Don't add the object file to the list of generated files if we are using + # visual studio and we are attaching the build rule to the cuda file. VS + # will add our object file to the linker automatically for us. + set(cuda_add_generated_file TRUE) + + if(NOT compile_to_ptx AND CMAKE_GENERATOR MATCHES "Visual Studio" AND CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE) + # Visual Studio 8 crashes when you close the solution when you don't add the object file. + if(NOT CMAKE_GENERATOR MATCHES "Visual Studio 8") + #message("Not adding ${generated_file}") + set(cuda_add_generated_file FALSE) + endif() + endif() + + if(cuda_add_generated_file) + list(APPEND _cuda_wrap_generated_files ${generated_file}) + endif() + + # Add the other files that we want cmake to clean on a cleanup ########## + list(APPEND CUDA_ADDITIONAL_CLEAN_FILES "${cmake_dependency_file}") + list(REMOVE_DUPLICATES CUDA_ADDITIONAL_CLEAN_FILES) + set(CUDA_ADDITIONAL_CLEAN_FILES ${CUDA_ADDITIONAL_CLEAN_FILES} CACHE INTERNAL "List of intermediate files that are part of the cuda dependency scanning.") + + endif(${file} MATCHES ".*\\.cu$" AND NOT _is_header) + endforeach(file) + + # Set the return parameter + set(${generated_files} ${_cuda_wrap_generated_files}) +endmacro(CUDA_WRAP_SRCS) + + +############################################################################### +############################################################################### +# ADD LIBRARY +############################################################################### +############################################################################### +macro(CUDA_ADD_LIBRARY cuda_target) + + CUDA_ADD_CUDA_INCLUDE_ONCE() + + # Separate the sources from the options + CUDA_GET_SOURCES_AND_OPTIONS(_sources _cmake_options _options ${ARGN}) + CUDA_BUILD_SHARED_LIBRARY(_cuda_shared_flag ${ARGN}) + # Create custom commands and targets for each file. + CUDA_WRAP_SRCS( ${cuda_target} OBJ _generated_files ${_sources} + ${_cmake_options} ${_cuda_shared_flag} + OPTIONS ${_options} ) + + # Add the library. + add_library(${cuda_target} ${_cmake_options} + ${_generated_files} + ${_sources} + ) + + target_link_libraries(${cuda_target} + ${CUDA_LIBRARIES} + ) + + # We need to set the linker language based on what the expected generated file + # would be. CUDA_C_OR_CXX is computed based on CUDA_HOST_COMPILATION_CPP. + set_target_properties(${cuda_target} + PROPERTIES + LINKER_LANGUAGE ${CUDA_C_OR_CXX} + ) + +endmacro(CUDA_ADD_LIBRARY cuda_target) + + +############################################################################### +############################################################################### +# ADD EXECUTABLE +############################################################################### +############################################################################### +macro(CUDA_ADD_EXECUTABLE cuda_target) + + CUDA_ADD_CUDA_INCLUDE_ONCE() + + # Separate the sources from the options + CUDA_GET_SOURCES_AND_OPTIONS(_sources _cmake_options _options ${ARGN}) + # Create custom commands and targets for each file. + CUDA_WRAP_SRCS( ${cuda_target} OBJ _generated_files ${_sources} OPTIONS ${_options} ) + + # Add the library. + add_executable(${cuda_target} ${_cmake_options} + ${_generated_files} + ${_sources} + ) + + target_link_libraries(${cuda_target} + ${CUDA_LIBRARIES} + ) + + # We need to set the linker language based on what the expected generated file + # would be. CUDA_C_OR_CXX is computed based on CUDA_HOST_COMPILATION_CPP. + set_target_properties(${cuda_target} + PROPERTIES + LINKER_LANGUAGE ${CUDA_C_OR_CXX} + ) + +endmacro(CUDA_ADD_EXECUTABLE cuda_target) + + +############################################################################### +############################################################################### +# CUDA COMPILE +############################################################################### +############################################################################### +macro(CUDA_COMPILE generated_files) + + # Separate the sources from the options + CUDA_GET_SOURCES_AND_OPTIONS(_sources _cmake_options _options ${ARGN}) + # Create custom commands and targets for each file. + CUDA_WRAP_SRCS( cuda_compile OBJ _generated_files ${_sources} ${_cmake_options} + OPTIONS ${_options} ) + + set( ${generated_files} ${_generated_files}) + +endmacro(CUDA_COMPILE) + + +############################################################################### +############################################################################### +# CUDA COMPILE PTX +############################################################################### +############################################################################### +macro(CUDA_COMPILE_PTX generated_files) + + # Separate the sources from the options + CUDA_GET_SOURCES_AND_OPTIONS(_sources _cmake_options _options ${ARGN}) + # Create custom commands and targets for each file. + CUDA_WRAP_SRCS( cuda_compile_ptx PTX _generated_files ${_sources} ${_cmake_options} + OPTIONS ${_options} ) + + set( ${generated_files} ${_generated_files}) + +endmacro(CUDA_COMPILE_PTX) + +############################################################################### +############################################################################### +# CUDA ADD CUFFT TO TARGET +############################################################################### +############################################################################### +macro(CUDA_ADD_CUFFT_TO_TARGET target) + if (CUDA_BUILD_EMULATION) + target_link_libraries(${target} ${CUDA_cufftemu_LIBRARY}) + else() + target_link_libraries(${target} ${CUDA_cufft_LIBRARY}) + endif() +endmacro() + +############################################################################### +############################################################################### +# CUDA ADD CUBLAS TO TARGET +############################################################################### +############################################################################### +macro(CUDA_ADD_CUBLAS_TO_TARGET target) + if (CUDA_BUILD_EMULATION) + target_link_libraries(${target} ${CUDA_cublasemu_LIBRARY}) + else() + target_link_libraries(${target} ${CUDA_cublas_LIBRARY}) + endif() +endmacro() + +############################################################################### +############################################################################### +# CUDA BUILD CLEAN TARGET +############################################################################### +############################################################################### +macro(CUDA_BUILD_CLEAN_TARGET) + # Call this after you add all your CUDA targets, and you will get a convience + # target. You should also make clean after running this target to get the + # build system to generate all the code again. + + set(cuda_clean_target_name clean_cuda_depends) + if (CMAKE_GENERATOR MATCHES "Visual Studio") + string(TOUPPER ${cuda_clean_target_name} cuda_clean_target_name) + endif() + add_custom_target(${cuda_clean_target_name} + COMMAND ${CMAKE_COMMAND} -E remove ${CUDA_ADDITIONAL_CLEAN_FILES}) + + # Clear out the variable, so the next time we configure it will be empty. + # This is useful so that the files won't persist in the list after targets + # have been removed. + set(CUDA_ADDITIONAL_CLEAN_FILES "" CACHE INTERNAL "List of intermediate files that are part of the cuda dependency scanning.") +endmacro(CUDA_BUILD_CLEAN_TARGET) diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA/make2cmake.cmake b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA/make2cmake.cmake new file mode 100644 index 0000000..7fce167 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA/make2cmake.cmake @@ -0,0 +1,79 @@ +# James Bigler, NVIDIA Corp (nvidia.com - jbigler) +# Abe Stephens, SCI Institute -- http://www.sci.utah.edu/~abe/FindCuda.html +# +# Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. +# +# Copyright (c) 2007-2009 +# Scientific Computing and Imaging Institute, University of Utah +# +# This code is licensed under the MIT License. See the FindCUDA.cmake script +# for the text of the license. + +# The MIT License +# +# License for the specific language governing rights and limitations under +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the "Software"), +# to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, +# and/or sell copies of the Software, and to permit persons to whom the +# Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. +# + +####################################################################### +# This converts a file written in makefile syntax into one that can be included +# by CMake. + +file(READ ${input_file} depend_text) + +if (${depend_text} MATCHES ".+") + + # message("FOUND DEPENDS") + + # Remember, four backslashes is escaped to one backslash in the string. + string(REGEX REPLACE "\\\\ " " " depend_text ${depend_text}) + + # This works for the nvcc -M generated dependency files. + string(REGEX REPLACE "^.* : " "" depend_text ${depend_text}) + string(REGEX REPLACE "[ \\\\]*\n" ";" depend_text ${depend_text}) + + set(dependency_list "") + + foreach(file ${depend_text}) + + string(REGEX REPLACE "^ +" "" file ${file}) + + if(NOT IS_DIRECTORY ${file}) + # If softlinks start to matter, we should change this to REALPATH. For now we need + # to flatten paths, because nvcc can generate stuff like /bin/../include instead of + # just /include. + get_filename_component(file_absolute "${file}" ABSOLUTE) + list(APPEND dependency_list "${file_absolute}") + endif(NOT IS_DIRECTORY ${file}) + + endforeach(file) + +else() + # message("FOUND NO DEPENDS") +endif() + +# Remove the duplicate entries and sort them. +list(REMOVE_DUPLICATES dependency_list) +list(SORT dependency_list) + +foreach(file ${dependency_list}) + set(cuda_nvcc_depend "${cuda_nvcc_depend} \"${file}\"\n") +endforeach() + +file(WRITE ${output_file} "# Generated by: make2cmake.cmake\nSET(CUDA_NVCC_DEPEND\n ${cuda_nvcc_depend})\n\n") diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA/parse_cubin.cmake b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA/parse_cubin.cmake new file mode 100644 index 0000000..2518c68 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA/parse_cubin.cmake @@ -0,0 +1,112 @@ +# James Bigler, NVIDIA Corp (nvidia.com - jbigler) +# Abe Stephens, SCI Institute -- http://www.sci.utah.edu/~abe/FindCuda.html +# +# Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. +# +# Copyright (c) 2007-2009 +# Scientific Computing and Imaging Institute, University of Utah +# +# This code is licensed under the MIT License. See the FindCUDA.cmake script +# for the text of the license. + +# The MIT License +# +# License for the specific language governing rights and limitations under +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the "Software"), +# to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, +# and/or sell copies of the Software, and to permit persons to whom the +# Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. +# + +####################################################################### +# Parses a .cubin file produced by nvcc and reports statistics about the file. + + +file(READ ${input_file} file_text) + +if (${file_text} MATCHES ".+") + + # Remember, four backslashes is escaped to one backslash in the string. + string(REGEX REPLACE ";" "\\\\;" file_text ${file_text}) + string(REGEX REPLACE "\ncode" ";code" file_text ${file_text}) + + list(LENGTH file_text len) + + foreach(line ${file_text}) + + # Only look at "code { }" blocks. + if(line MATCHES "^code") + + # Break into individual lines. + string(REGEX REPLACE "\n" ";" line ${line}) + + foreach(entry ${line}) + + # Extract kernel names. + if (${entry} MATCHES "[^g]name = ([^ ]+)") + string(REGEX REPLACE ".* = ([^ ]+)" "\\1" entry ${entry}) + + # Check to see if the kernel name starts with "_" + set(skip FALSE) + # if (${entry} MATCHES "^_") + # Skip the rest of this block. + # message("Skipping ${entry}") + # set(skip TRUE) + # else (${entry} MATCHES "^_") + message("Kernel: ${entry}") + # endif (${entry} MATCHES "^_") + + endif(${entry} MATCHES "[^g]name = ([^ ]+)") + + # Skip the rest of the block if necessary + if(NOT skip) + + # Registers + if (${entry} MATCHES "reg([ ]+)=([ ]+)([^ ]+)") + string(REGEX REPLACE ".*([ ]+)=([ ]+)([^ ]+)" "\\3" entry ${entry}) + message("Registers: ${entry}") + endif() + + # Local memory + if (${entry} MATCHES "lmem([ ]+)=([ ]+)([^ ]+)") + string(REGEX REPLACE ".*([ ]+)=([ ]+)([^ ]+)" "\\3" entry ${entry}) + message("Local: ${entry}") + endif() + + # Shared memory + if (${entry} MATCHES "smem([ ]+)=([ ]+)([^ ]+)") + string(REGEX REPLACE ".*([ ]+)=([ ]+)([^ ]+)" "\\3" entry ${entry}) + message("Shared: ${entry}") + endif() + + if (${entry} MATCHES "^}") + message("") + endif() + + endif(NOT skip) + + + endforeach(entry) + + endif(line MATCHES "^code") + + endforeach(line) + +else() + # message("FOUND NO DEPENDS") +endif() + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA/run_nvcc.cmake b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA/run_nvcc.cmake new file mode 100644 index 0000000..dce98e3 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindCUDA/run_nvcc.cmake @@ -0,0 +1,280 @@ +# James Bigler, NVIDIA Corp (nvidia.com - jbigler) +# +# Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. +# +# This code is licensed under the MIT License. See the FindCUDA.cmake script +# for the text of the license. + +# The MIT License +# +# License for the specific language governing rights and limitations under +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the "Software"), +# to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, +# and/or sell copies of the Software, and to permit persons to whom the +# Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. + + +########################################################################## +# This file runs the nvcc commands to produce the desired output file along with +# the dependency file needed by CMake to compute dependencies. In addition the +# file checks the output of each command and if the command fails it deletes the +# output files. + +# Input variables +# +# verbose:BOOL=<> OFF: Be as quiet as possible (default) +# ON : Describe each step +# +# build_configuration:STRING=<> Typically one of Debug, MinSizeRel, Release, or +# RelWithDebInfo, but it should match one of the +# entries in CUDA_HOST_FLAGS. This is the build +# configuration used when compiling the code. If +# blank or unspecified Debug is assumed as this is +# what CMake does. +# +# generated_file:STRING=<> File to generate. This argument must be passed in. +# +# generated_cubin_file:STRING=<> File to generate. This argument must be passed +# in if build_cubin is true. + +if(NOT generated_file) + message(FATAL_ERROR "You must specify generated_file on the command line") +endif() + +# Set these up as variables to make reading the generated file easier +set(CMAKE_COMMAND "@CMAKE_COMMAND@") +set(source_file "@source_file@") +set(NVCC_generated_dependency_file "@NVCC_generated_dependency_file@") +set(cmake_dependency_file "@cmake_dependency_file@") +set(CUDA_make2cmake "@CUDA_make2cmake@") +set(CUDA_parse_cubin "@CUDA_parse_cubin@") +set(build_cubin @build_cubin@) +# We won't actually use these variables for now, but we need to set this, in +# order to force this file to be run again if it changes. +set(generated_file_path "@generated_file_path@") +set(generated_file_internal "@generated_file@") +set(generated_cubin_file_internal "@generated_cubin_file@") + +set(CUDA_NVCC_EXECUTABLE "@CUDA_NVCC_EXECUTABLE@") +set(CUDA_NVCC_FLAGS "@CUDA_NVCC_FLAGS@;;@CUDA_WRAP_OPTION_NVCC_FLAGS@") +@CUDA_NVCC_FLAGS_CONFIG@ +set(nvcc_flags @nvcc_flags@) +set(CUDA_NVCC_INCLUDE_ARGS "@CUDA_NVCC_INCLUDE_ARGS@") +set(format_flag "@format_flag@") + +if(build_cubin AND NOT generated_cubin_file) + message(FATAL_ERROR "You must specify generated_cubin_file on the command line") +endif() + +# This is the list of host compilation flags. It C or CXX should already have +# been chosen by FindCUDA.cmake. +@CUDA_HOST_FLAGS@ + +# Take the compiler flags and package them up to be sent to the compiler via -Xcompiler +set(nvcc_host_compiler_flags "") +# If we weren't given a build_configuration, use Debug. +if(NOT build_configuration) + set(build_configuration Debug) +endif() +string(TOUPPER "${build_configuration}" build_configuration) +#message("CUDA_NVCC_HOST_COMPILER_FLAGS = ${CUDA_NVCC_HOST_COMPILER_FLAGS}") +foreach(flag ${CMAKE_HOST_FLAGS} ${CMAKE_HOST_FLAGS_${build_configuration}}) + # Extra quotes are added around each flag to help nvcc parse out flags with spaces. + set(nvcc_host_compiler_flags "${nvcc_host_compiler_flags},\"${flag}\"") +endforeach() +if (nvcc_host_compiler_flags) + set(nvcc_host_compiler_flags "-Xcompiler" ${nvcc_host_compiler_flags}) +endif() +#message("nvcc_host_compiler_flags = \"${nvcc_host_compiler_flags}\"") +# Add the build specific configuration flags +list(APPEND CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS_${build_configuration}}) + +if(DEFINED CCBIN) + set(CCBIN -ccbin "${CCBIN}") +endif() + +# cuda_execute_process - Executes a command with optional command echo and status message. +# +# status - Status message to print if verbose is true +# command - COMMAND argument from the usual execute_process argument structure +# ARGN - Remaining arguments are the command with arguments +# +# CUDA_result - return value from running the command +# +# Make this a macro instead of a function, so that things like RESULT_VARIABLE +# and other return variables are present after executing the process. +macro(cuda_execute_process status command) + set(_command ${command}) + if(NOT _command STREQUAL "COMMAND") + message(FATAL_ERROR "Malformed call to cuda_execute_process. Missing COMMAND as second argument. (command = ${command})") + endif() + if(verbose) + execute_process(COMMAND "${CMAKE_COMMAND}" -E echo -- ${status}) + # Now we need to build up our command string. We are accounting for quotes + # and spaces, anything else is left up to the user to fix if they want to + # copy and paste a runnable command line. + set(cuda_execute_process_string) + foreach(arg ${ARGN}) + # If there are quotes, excape them, so they come through. + string(REPLACE "\"" "\\\"" arg ${arg}) + # Args with spaces need quotes around them to get them to be parsed as a single argument. + if(arg MATCHES " ") + list(APPEND cuda_execute_process_string "\"${arg}\"") + else() + list(APPEND cuda_execute_process_string ${arg}) + endif() + endforeach() + # Echo the command + execute_process(COMMAND ${CMAKE_COMMAND} -E echo ${cuda_execute_process_string}) + endif(verbose) + # Run the command + execute_process(COMMAND ${ARGN} RESULT_VARIABLE CUDA_result ) +endmacro() + +# Delete the target file +cuda_execute_process( + "Removing ${generated_file}" + COMMAND "${CMAKE_COMMAND}" -E remove "${generated_file}" + ) + +# For CUDA 2.3 and below, -G -M doesn't work, so remove the -G flag +# for dependency generation and hope for the best. +set(depends_CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS}") +set(CUDA_VERSION @CUDA_VERSION@) +if(CUDA_VERSION VERSION_LESS "3.0") + cmake_policy(PUSH) + # CMake policy 0007 NEW states that empty list elements are not + # ignored. I'm just setting it to avoid the warning that's printed. + cmake_policy(SET CMP0007 NEW) + # Note that this will remove all occurances of -G. + list(REMOVE_ITEM depends_CUDA_NVCC_FLAGS "-G") + cmake_policy(POP) +endif() + +# nvcc doesn't define __CUDACC__ for some reason when generating dependency files. This +# can cause incorrect dependencies when #including files based on this macro which is +# defined in the generating passes of nvcc invokation. We will go ahead and manually +# define this for now until a future version fixes this bug. +set(CUDACC_DEFINE -D__CUDACC__) + +# Generate the dependency file +cuda_execute_process( + "Generating dependency file: ${NVCC_generated_dependency_file}" + COMMAND "${CUDA_NVCC_EXECUTABLE}" + -M + ${CUDACC_DEFINE} + "${source_file}" + -o "${NVCC_generated_dependency_file}" + ${CCBIN} + ${nvcc_flags} + ${nvcc_host_compiler_flags} + ${depends_CUDA_NVCC_FLAGS} + -DNVCC + ${CUDA_NVCC_INCLUDE_ARGS} + ) + +if(CUDA_result) + message(FATAL_ERROR "Error generating ${generated_file}") +endif() + +# Generate the cmake readable dependency file to a temp file. Don't put the +# quotes just around the filenames for the input_file and output_file variables. +# CMake will pass the quotes through and not be able to find the file. +cuda_execute_process( + "Generating temporary cmake readable file: ${cmake_dependency_file}.tmp" + COMMAND "${CMAKE_COMMAND}" + -D "input_file:FILEPATH=${NVCC_generated_dependency_file}" + -D "output_file:FILEPATH=${cmake_dependency_file}.tmp" + -P "${CUDA_make2cmake}" + ) + +if(CUDA_result) + message(FATAL_ERROR "Error generating ${generated_file}") +endif() + +# Copy the file if it is different +cuda_execute_process( + "Copy if different ${cmake_dependency_file}.tmp to ${cmake_dependency_file}" + COMMAND "${CMAKE_COMMAND}" -E copy_if_different "${cmake_dependency_file}.tmp" "${cmake_dependency_file}" + ) + +if(CUDA_result) + message(FATAL_ERROR "Error generating ${generated_file}") +endif() + +# Delete the temporary file +cuda_execute_process( + "Removing ${cmake_dependency_file}.tmp and ${NVCC_generated_dependency_file}" + COMMAND "${CMAKE_COMMAND}" -E remove "${cmake_dependency_file}.tmp" "${NVCC_generated_dependency_file}" + ) + +if(CUDA_result) + message(FATAL_ERROR "Error generating ${generated_file}") +endif() + +# Generate the code +cuda_execute_process( + "Generating ${generated_file}" + COMMAND "${CUDA_NVCC_EXECUTABLE}" + "${source_file}" + ${format_flag} -o "${generated_file}" + ${CCBIN} + ${nvcc_flags} + ${nvcc_host_compiler_flags} + ${CUDA_NVCC_FLAGS} + -DNVCC + ${CUDA_NVCC_INCLUDE_ARGS} + ) + +if(CUDA_result) + # Since nvcc can sometimes leave half done files make sure that we delete the output file. + cuda_execute_process( + "Removing ${generated_file}" + COMMAND "${CMAKE_COMMAND}" -E remove "${generated_file}" + ) + message(FATAL_ERROR "Error generating file ${generated_file}") +else() + if(verbose) + message("Generated ${generated_file} successfully.") + endif() +endif() + +# Cubin resource report commands. +if( build_cubin ) + # Run with -cubin to produce resource usage report. + cuda_execute_process( + "Generating ${generated_cubin_file}" + COMMAND "${CUDA_NVCC_EXECUTABLE}" + "${source_file}" + ${CUDA_NVCC_FLAGS} + ${nvcc_flags} + ${CCBIN} + ${nvcc_host_compiler_flags} + -DNVCC + -cubin + -o "${generated_cubin_file}" + ${CUDA_NVCC_INCLUDE_ARGS} + ) + + # Execute the parser script. + cuda_execute_process( + "Executing the parser script" + COMMAND "${CMAKE_COMMAND}" + -D "input_file:STRING=${generated_cubin_file}" + -P "${CUDA_parse_cubin}" + ) + +endif( build_cubin ) diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake new file mode 100644 index 0000000..cac56d8 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake @@ -0,0 +1,81 @@ +############################################################################### +# +# CMake script for finding the Eigen library. +# +# http://eigen.tuxfamily.org/index.php?title=Main_Page +# +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD +# license. +# +# +# Input variables: +# +# - Eigen_ROOT_DIR (optional): When specified, header files and libraries +# will be searched for in `${Eigen_ROOT_DIR}/include` and +# `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order +# will be ignored. When unspecified, the default CMake search order is used. +# This variable can be specified either as a CMake or environment variable. +# If both are set, preference is given to the CMake variable. +# Use this variable for finding packages installed in a nonstandard location, +# or for enforcing that one of multiple package installations is picked up. +# +# Cache variables (not intended to be used in CMakeLists.txt files) +# +# - Eigen_INCLUDE_DIR: Absolute path to package headers. +# +# +# Output variables: +# +# - Eigen_FOUND: Boolean that indicates if the package was found +# - Eigen_INCLUDE_DIRS: Paths to the necessary header files +# - Eigen_VERSION: Version of Eigen library found +# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen +# +# +# Example usage: +# +# # Passing the version means Eigen_FOUND will only be TRUE if a +# # version >= the provided version is found. +# find_package(Eigen 3.1.2) +# if(NOT Eigen_FOUND) +# # Error handling +# endif() +# ... +# add_definitions(${Eigen_DEFINITIONS}) +# ... +# include_directories(${Eigen_INCLUDE_DIRS} ...) +# +############################################################################### + +find_package(PkgConfig) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + +set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3/") +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" + "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" + PATH_SUFFIXES eigen3 include/eigen3 include) + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") +endif(EIGEN_FOUND) + + +set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +set(Eigen_FOUND ${EIGEN_FOUND}) +set(Eigen_VERSION ${EIGEN_VERSION}) +set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake~ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake~ new file mode 100644 index 0000000..8587367 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/CMakeModules/FindEigen.cmake~ @@ -0,0 +1,81 @@ +############################################################################### +# +# CMake script for finding the Eigen library. +# +# http://eigen.tuxfamily.org/index.php?title=Main_Page +# +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD +# license. +# +# +# Input variables: +# +# - Eigen_ROOT_DIR (optional): When specified, header files and libraries +# will be searched for in `${Eigen_ROOT_DIR}/include` and +# `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order +# will be ignored. When unspecified, the default CMake search order is used. +# This variable can be specified either as a CMake or environment variable. +# If both are set, preference is given to the CMake variable. +# Use this variable for finding packages installed in a nonstandard location, +# or for enforcing that one of multiple package installations is picked up. +# +# Cache variables (not intended to be used in CMakeLists.txt files) +# +# - Eigen_INCLUDE_DIR: Absolute path to package headers. +# +# +# Output variables: +# +# - Eigen_FOUND: Boolean that indicates if the package was found +# - Eigen_INCLUDE_DIRS: Paths to the necessary header files +# - Eigen_VERSION: Version of Eigen library found +# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen +# +# +# Example usage: +# +# # Passing the version means Eigen_FOUND will only be TRUE if a +# # version >= the provided version is found. +# find_package(Eigen 3.1.2) +# if(NOT Eigen_FOUND) +# # Error handling +# endif() +# ... +# add_definitions(${Eigen_DEFINITIONS}) +# ... +# include_directories(${Eigen_INCLUDE_DIRS} ...) +# +############################################################################### + +find_package(PkgConfig) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + + +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" + "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" + PATH_SUFFIXES eigen3 include/eigen3 include) + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") +endif(EIGEN_FOUND) + + +set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +set(Eigen_FOUND ${EIGEN_FOUND}) +set(Eigen_VERSION ${EIGEN_VERSION}) +set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/cfg/local_sensing_node.cfg b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/cfg/local_sensing_node.cfg new file mode 100755 index 0000000..ac82ee4 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/cfg/local_sensing_node.cfg @@ -0,0 +1,15 @@ +#!/usr/bin/env python +PACKAGE = "local_sensing_node" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("tx", double_t, 0, "tx", 0.0, -1000.0, 1000.0) +gen.add("ty", double_t, 0, "ty", 0.0, -1000.0, 1000.0) +gen.add("tz", double_t, 0, "tz", 0.0, -1000.0, 1000.0) +gen.add("axis_x", double_t, 0, "axis_x", 0.0, -360.0, 360.0) +gen.add("axis_y", double_t, 0, "axis_y", 0.0, -360.0, 360.0) +gen.add("axis_z", double_t, 0, "axis_z", 0.0, -360.0, 360.0) + +exit(gen.generate(PACKAGE, "local_sensing_node", "local_sensing_node")) \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/package.xml b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/package.xml new file mode 100644 index 0000000..ae0110c --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/package.xml @@ -0,0 +1,38 @@ + + + local_sensing_node + 0.1.0 + + render depth from depth + + Matia Pizzoli + Matia Pizzoli + GPLv3 + + + catkin + + + cmake_modules + roscpp + roslib + svo_msgs + cv_bridge + image_transport + vikit_ros + pcl_ros + dynamic_reconfigure + quadrotor_msgs + + + roscpp + roslib + svo_msgs + cv_bridge + image_transport + vikit_ros + pcl_ros + dynamic_reconfigure + quadrotor_msgs + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/params/camera.yaml b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/params/camera.yaml new file mode 100644 index 0000000..8e375b2 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/params/camera.yaml @@ -0,0 +1,6 @@ +cam_width: 640 +cam_height: 480 +cam_fx: 387.229248046875 +cam_fy: 387.229248046875 +cam_cx: 321.04638671875 +cam_cy: 243.44969177246094 diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/AlignError.h b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/AlignError.h new file mode 100644 index 0000000..04d4076 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/AlignError.h @@ -0,0 +1,86 @@ +#include +#include + +struct AlignError { + AlignError( const Eigen::Quaterniond camera_pose, const Eigen::Vector3d camera_trans, + const Eigen::Quaterniond velodyne_pose, const Eigen::Vector3d velodyne_trans) + { + camera_q[0] = camera_pose.x(); + camera_q[1] = camera_pose.y(); + camera_q[2] = camera_pose.z(); + camera_q[3] = camera_pose.w(); + camera_t[0] = camera_trans.x(); + camera_t[1] = camera_trans.y(); + camera_t[2] = camera_trans.z(); + + velodyne_q[0] = velodyne_pose.x(); + velodyne_q[1] = velodyne_pose.y(); + velodyne_q[2] = velodyne_pose.z(); + velodyne_q[3] = velodyne_pose.w(); + velodyne_t[0] = velodyne_trans.x(); + velodyne_t[1] = velodyne_trans.y(); + velodyne_t[2] = velodyne_trans.z(); + } + + // Factory to hide the construction of the CostFunction object from the client code. + static ceres::CostFunction* Create( const Eigen::Quaterniond camera_pose, const Eigen::Vector3d camera_trans, + const Eigen::Quaterniond velodyne_pose, const Eigen::Vector3d velodyne_trans) + { + return (new ceres::AutoDiffCostFunction( + new AlignError(camera_pose, camera_trans, velodyne_pose, velodyne_trans))); + } + + template + bool operator()(const T* const world_rotation, const T* const world_translation, + const T* const v2c_rotation, const T* const v2c_translation, + T* residuals) const + { + Eigen::Quaternion q_world = Eigen::Map< const Eigen::Quaternion >(world_rotation); + Eigen::Matrix t_world = Eigen::Map< const Eigen::Matrix >(world_translation); + + Eigen::Quaternion q_v2c = Eigen::Map< const Eigen::Quaternion >(v2c_rotation); + Eigen::Matrix t_v2c = Eigen::Map< const Eigen::Matrix >(v2c_translation); + + Eigen::Quaternion q_c; + Eigen::Matrix t_c; + q_c.x() = T(camera_q[0]); + q_c.y() = T(camera_q[1]); + q_c.z() = T(camera_q[2]); + q_c.w() = T(camera_q[3]); + t_c << T(camera_t[0]), T(camera_t[1]), T(camera_t[2]); + + Eigen::Quaternion q_v; + Eigen::Matrix t_v; + q_v.x() = T(velodyne_q[0]); + q_v.y() = T(velodyne_q[1]); + q_v.z() = T(velodyne_q[2]); + q_v.w() = T(velodyne_q[3]); + t_v << T(velodyne_t[0]), T(velodyne_t[1]), T(velodyne_t[2]); + + Eigen::Quaternion q_left; + Eigen::Matrix t_left; + + q_left = q_world * q_c * q_v2c; + t_left = q_world * q_c * t_v2c + q_world * t_c + t_world; + + Eigen::Quaternion q_diff; + Eigen::Matrix t_diff; + q_diff = q_left * q_v.inverse(); + // t_diff = t_left - q_left * q_v.inverse() * t_v; + t_diff = t_left - t_v; + + residuals[0] = q_diff.x(); + residuals[1] = q_diff.y(); + residuals[2] = q_diff.z(); + residuals[3] = t_diff(0); + residuals[4] = t_diff(1); + residuals[5] = t_diff(2); + + return true; + } + + double camera_q[4]; + double camera_t[3]; + double velodyne_q[4]; + double velodyne_t[3]; +}; diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/ceres_extensions.h b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/ceres_extensions.h new file mode 100644 index 0000000..a79fcf4 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/ceres_extensions.h @@ -0,0 +1,161 @@ +// +// ceres_extensions.h +// Bundle_Adjust_Test +// +// Created by Lloyd Hughes on 2014/04/11. +// Copyright (c) 2014 Lloyd Hughes. All rights reserved. +// hughes.lloyd@gmail.com +// + +#ifndef CERES_EXTENSIONS_H +#define CERES_EXTENSIONS_H + +#include +#include +#include + +namespace ceres_ext { + + // Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x + // with * being the quaternion multiplication operator. Here we assume + // that the first element of the quaternion vector is the real (cos + // theta) part. + class EigenQuaternionParameterization : public ceres::LocalParameterization + { + public: + virtual ~EigenQuaternionParameterization() {} + + virtual bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const + { + const Eigen::Map x(x_raw); + const Eigen::Map delta(delta_raw); + + Eigen::Map x_plus_delta(x_plus_delta_raw); + + const double delta_norm = delta.norm(); + if ( delta_norm > 0.0 ) + { + const double sin_delta_by_delta = sin(delta_norm) / delta_norm; + Eigen::Quaterniond tmp( cos(delta_norm), sin_delta_by_delta*delta[0], sin_delta_by_delta*delta[1], sin_delta_by_delta*delta[2] ); + + x_plus_delta = tmp*x; + } + else + { + x_plus_delta = x; + } + return true; + } + + virtual bool ComputeJacobian(const double* x, double* jacobian) const + { + jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT x + jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT y + jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT z + jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT w + return true; + } + + virtual int GlobalSize() const { return 4; } + virtual int LocalSize() const { return 3; } + + }; + + template inline + void EigenQuaternionToScaledRotation(const T q[4], T R[3 * 3]) { + EigenQuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); + } + + template inline + void EigenQuaternionToScaledRotation(const T q[4], + const ceres::MatrixAdapter& R) { + // Make convenient names for elements of q. + T a = q[3]; + T b = q[0]; + T c = q[1]; + T d = q[2]; + // This is not to eliminate common sub-expression, but to + // make the lines shorter so that they fit in 80 columns! + T aa = a * a; + T ab = a * b; + T ac = a * c; + T ad = a * d; + T bb = b * b; + T bc = b * c; + T bd = b * d; + T cc = c * c; + T cd = c * d; + T dd = d * d; + + R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT + R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT + R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT + } + + template inline + void EigenQuaternionToRotation(const T q[4], T R[3 * 3]) { + EigenQuaternionToRotation(q, RowMajorAdapter3x3(R)); + } + + template inline + void EigenQuaternionToRotation(const T q[4], + const ceres::MatrixAdapter& R) { + EigenQuaternionToScaledRotation(q, R); + + T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; + CHECK_NE(normalizer, T(0)); + normalizer = T(1) / normalizer; + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + R(i, j) *= normalizer; + } + } + } + + template inline + void EigenUnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + const T t2 = q[3] * q[0]; + const T t3 = q[3] * q[1]; + const T t4 = q[3] * q[2]; + const T t5 = -q[0] * q[0]; + const T t6 = q[0] * q[1]; + const T t7 = q[0] * q[2]; + const T t8 = -q[1] * q[1]; + const T t9 = q[1] * q[2]; + const T t1 = -q[2] * q[2]; + result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT + result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT + result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT + } + + template inline + void EigenQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + // 'scale' is 1 / norm(q). + const T scale = T(1) / sqrt(q[0] * q[0] + + q[1] * q[1] + + q[2] * q[2] + + q[3] * q[3]); + + // Make unit-norm version of q. + const T unit[4] = { + scale * q[0], + scale * q[1], + scale * q[2], + scale * q[3], + }; + + EigenUnitQuaternionRotatePoint(unit, pt, result); + } + + template inline + void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) { + zw[0] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; + zw[1] = - z[0] * w[2] + z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; + zw[2] = z[0] * w[1] - z[1] * w[0] + z[2] * w[3] + z[3] * w[2]; + zw[3] = - z[0] * w[0] - z[1] * w[1] - z[2] * w[2] + z[3] * w[3]; + } + +} + +#endif \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/csv_convert.py b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/csv_convert.py new file mode 100644 index 0000000..0040baa --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/csv_convert.py @@ -0,0 +1,15 @@ +import csv +file_location = '/home/wang/bag/banchmark/EuRoC/ViconRoom101/state_groundtruth_estimate0/data.csv' +with open('/home/wang/bag/banchmark/EuRoC/ViconRoom101/state_groundtruth_estimate0/data.txt', 'w') as txt_f: + with open(file_location) as f: + f_csv = csv.reader(f) + headers = next(f_csv) + for row in f_csv: + txt_f.write('%lf\n'% (float(row[0]) / 1000000000.0) ) + txt_f.write('%lf\n'% (float(row[1])) ) + txt_f.write('%lf\n'% (float(row[2])) ) + txt_f.write('%lf\n'% (float(row[3])) ) + txt_f.write('%lf\n'% (float(row[4])) ) + txt_f.write('%lf\n'% (float(row[5])) ) + txt_f.write('%lf\n'% (float(row[6])) ) + txt_f.write('%lf\n'% (float(row[7])) ) \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/cuda_exception.cuh b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/cuda_exception.cuh new file mode 100644 index 0000000..cf8213f --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/cuda_exception.cuh @@ -0,0 +1,45 @@ +// This file is part of REMODE - REgularized MOnocular Depth Estimation. +// +// Copyright (C) 2014 Matia Pizzoli +// Robotics and Perception Group, University of Zurich, Switzerland +// http://rpg.ifi.uzh.ch +// +// REMODE is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// REMODE 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 General Public License +// along with this program. If not, see . + +#ifndef RMD_CUDA_EXCEPTION_CUH_ +#define RMD_CUDA_EXCEPTION_CUH_ + +#include +#include + + +struct CudaException : public std::exception +{ + CudaException(const std::string& what, cudaError err) + : what_(what), err_(err) {} + virtual ~CudaException() throw() {} + virtual const char* what() const throw() + { + std::stringstream description; + description << "CudaException: " << what_ << std::endl; + if(err_ != cudaSuccess) + { + description << "cudaError code: " << cudaGetErrorString(err_); + description << " (" << err_ << ")" << std::endl; + } + return description.str().c_str(); + } + std::string what_; + cudaError err_; +}; + +#endif /* RMD_CUDA_EXCEPTION_CUH_ */ diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/depth_render.cu b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/depth_render.cu new file mode 100644 index 0000000..1836780 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/depth_render.cu @@ -0,0 +1,197 @@ +#include "depth_render.cuh" +__global__ void render(float3 *data_devptr, Parameter *para_devptr, DeviceImage *depth_devptr) +{ + const int index = threadIdx.x + blockIdx.x * blockDim.x; + const Parameter para = *para_devptr; + if(index >= para.point_number) + return; + float3 my_point = data_devptr[index]; + + //transform + float3 trans_point; + trans_point.x = my_point.x * para.r[0][0] + my_point.y * para.r[0][1] + my_point.z * para.r[0][2] + para.t[0]; + trans_point.y = my_point.x * para.r[1][0] + my_point.y * para.r[1][1] + my_point.z * para.r[1][2] + para.t[1]; + trans_point.z = my_point.x * para.r[2][0] + my_point.y * para.r[2][1] + my_point.z * para.r[2][2] + para.t[2]; + + if(trans_point.z <= 0.0f) + return; + + //project + int2 projected; + projected.x = trans_point.x / trans_point.z * para.fx + para.cx + 0.5; + projected.y = trans_point.y / trans_point.z * para.fy + para.cy + 0.5; + if(projected.x < 0 || projected.x >= para.width || projected.y < 0 || projected.y >= para.height) + return; + + // float dist = length(trans_point); + float dist = trans_point.z; + int dist_mm = dist * 1000.0f + 0.5f; + + //int r = 0.0173 * para.fx / dist + 0.5f; +// int r = 0.0473 * para.fx / dist + 0.5f; + int r = 0.0573 * para.fx / dist + 0.5f; + for(int i = -r; i <= r; i++) + for(int j = -r; j <= r; j++) + { + int to_x = projected.x + j; + int to_y = projected.y + i; + if(to_x < 0 || to_x >= para.width || to_y < 0 || to_y >= para.height) + continue; + int *dist_ptr = &(depth_devptr->atXY(to_x, to_y)); + atomicMin(dist_ptr, dist_mm); + } +} + +__global__ void depth_initial(DeviceImage *depth_devptr) +{ + const int x = threadIdx.x + blockIdx.x * blockDim.x; + const int y = threadIdx.y + blockIdx.y * blockDim.y; + int width = depth_devptr->width; + int height = depth_devptr->height; + + if(x >= width || y >= height) + return; + + depth_devptr->atXY(x,y) = 999999; +} + +DepthRender::DepthRender(): + cloud_size(0), + host_cloud_ptr(NULL), + dev_cloud_ptr(NULL), + has_devptr(false) +{ +} + +DepthRender::~DepthRender() +{ + if(has_devptr) + { + free(host_cloud_ptr); + cudaFree(dev_cloud_ptr); + cudaFree(parameter_devptr); + } +} + +void DepthRender::set_para(float _fx, float _fy, float _cx, float _cy, int _width, int _height) +{ + parameter.fx = _fx; + parameter.fy = _fy; + parameter.cx = _cx; + parameter.cy = _cy; + parameter.width = _width; + parameter.height = _height; +} + +void DepthRender::set_data(vector &cloud_data) +{ + cloud_size = cloud_data.size() / 3; + parameter.point_number = cloud_size; + + host_cloud_ptr = (float3*) malloc(cloud_size * sizeof(float3)); + for(int i = 0; i < cloud_size; i++) + host_cloud_ptr[i] = make_float3(cloud_data[3*i], cloud_data[3*i+1], cloud_data[3*i+2]); + + cudaError err = cudaMalloc(&dev_cloud_ptr, cloud_size * sizeof(float3)); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to allocate linear memory.", err); + err = cudaMemcpy( + dev_cloud_ptr, + host_cloud_ptr, + cloud_size * sizeof(float3), + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to copy data from host to device.", err); + + err = cudaMalloc(¶meter_devptr, sizeof(Parameter)); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to allocate linear memory.", err); + err = cudaMemcpy( + parameter_devptr, + ¶meter, + sizeof(Parameter), + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to copy data from host to device.", err); + + has_devptr = true; + + //printf("load points done!\n"); +} + +/*void DepthRender::render_pose( Matrix3d &rotation, Vector3d &translation, int *host_ptr) +{ + for(int i = 0; i < 3; i++) + { + parameter.t[i] = translation(i); + for(int j = 0; j < 3; j++) + { + parameter.r[i][j] = rotation(i,j); + } + } + cudaError err = cudaMemcpy( + parameter_devptr, + ¶meter, + sizeof(Parameter), + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to copy data from host to device.", err); + + DeviceImage depth_output(parameter.width, parameter.height); + depth_output.zero(); + + dim3 depth_block; + dim3 depth_grid; + depth_block.x = 16; + depth_block.y = 16; + depth_grid.x = (parameter.width + depth_block.x - 1 ) / depth_block.x; + depth_grid.y = (parameter.height + depth_block.y - 1 ) / depth_block.y; + depth_initial<<>>(depth_output.dev_ptr); + + dim3 render_block; + dim3 render_grid; + render_block.x = 64; + render_grid.x = (cloud_size + render_block.x - 1) / render_block.x; + render<<>>(dev_cloud_ptr, parameter_devptr, depth_output.dev_ptr); + + depth_output.getDevData(host_ptr); +} +*/ +//void DepthRender::render_pose( Matrix4d &transformation, int *host_ptr) +void DepthRender::render_pose( double * transformation, int *host_ptr) +{ + for(int i = 0; i < 3; i++) + { + parameter.t[i] = transformation[4 * i + 3];//transformation(i,3); + for(int j = 0; j < 3; j++) + { + parameter.r[i][j] = transformation[4 * i + j];//transformation(i,j); + } + } + cudaError err = cudaMemcpy( + parameter_devptr, + ¶meter, + sizeof(Parameter), + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("DeviceLinear: unable to copy data from host to device.", err); + + DeviceImage depth_output(parameter.width, parameter.height); + depth_output.zero(); + + dim3 depth_block; + dim3 depth_grid; + depth_block.x = 16; + depth_block.y = 16; + depth_grid.x = (parameter.width + depth_block.x - 1 ) / depth_block.x; + depth_grid.y = (parameter.height + depth_block.y - 1 ) / depth_block.y; + depth_initial<<>>(depth_output.dev_ptr); + + dim3 render_block; + dim3 render_grid; + render_block.x = 64; + render_grid.x = (cloud_size + render_block.x - 1) / render_block.x; + render<<>>(dev_cloud_ptr, parameter_devptr, depth_output.dev_ptr); + + depth_output.getDevData(host_ptr); +} \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/depth_render.cuh b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/depth_render.cuh new file mode 100644 index 0000000..26b8518 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/depth_render.cuh @@ -0,0 +1,52 @@ +#ifndef DEPTH_RENDER_CUH +#define DEPTH_RENDER_CUH + +#include +#include +#include +#include +#include +#include +//#include + +#include "device_image.cuh" +#include "cuda_exception.cuh" +#include "helper_math.h" + +using namespace std; +//using namespace Eigen; + +struct Parameter +{ + int point_number; + float fx, fy, cx, cy; + int width, height; + float r[3][3]; + float t[3]; +}; + +class DepthRender +{ +public: + DepthRender(); + void set_para(float _fx, float _fy, float _cx, float _cy, int _width, int _height); + ~DepthRender(); + void set_data(vector &cloud_data); + //void render_pose( Matrix3d &rotation, Vector3d &translation, int *host_ptr); + //void render_pose( Matrix4d &transformation, int *host_ptr); + void render_pose( double * transformation, int *host_ptr); + +private: + int cloud_size; + + //data + float3 *host_cloud_ptr; + float3 *dev_cloud_ptr; + bool has_devptr; + + //camera + Parameter parameter; + Parameter* parameter_devptr; +}; + +#endif \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/device_image.cuh b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/device_image.cuh new file mode 100644 index 0000000..84e77ef --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/device_image.cuh @@ -0,0 +1,179 @@ +// This file is part of REMODE - REgularized MOnocular Depth Estimation. +// +// Copyright (C) 2014 Matia Pizzoli +// Robotics and Perception Group, University of Zurich, Switzerland +// http://rpg.ifi.uzh.ch +// +// REMODE is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// REMODE 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 General Public License +// along with this program. If not, see . + +#ifndef DEVICE_IMAGE_CUH +#define DEVICE_IMAGE_CUH + +#include +#include +#include "cuda_exception.cuh" + +struct Size +{ + int width; + int height; +}; + +template +struct DeviceImage +{ + __host__ + DeviceImage(size_t width, size_t height) + : width(width), + height(height) + { + cudaError err = cudaMallocPitch( + &data, + &pitch, + width*sizeof(ElementType), + height); + if(err != cudaSuccess) + throw CudaException("Image: unable to allocate pitched memory.", err); + + stride = pitch / sizeof(ElementType); + + err = cudaMalloc( + &dev_ptr, + sizeof(*this)); + if(err != cudaSuccess) + throw CudaException("DeviceData, cannot allocate device memory to store image parameters.", err); + + err = cudaMemcpy( + dev_ptr, + this, + sizeof(*this), + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("DeviceData, cannot copy image parameters to device memory.", err); + } + + __device__ + ElementType & operator()(size_t x, size_t y) + { + return atXY(x, y); + } + + __device__ + const ElementType & operator()(size_t x, size_t y) const + { + return atXY(x, y); + } + + __device__ + ElementType & atXY(size_t x, size_t y) + { + return data[y*stride+x]; + } + + __device__ + const ElementType & atXY(size_t x, size_t y) const + { + return data[y*stride+x]; + } + + /// Upload aligned_data_row_major to device memory + __host__ + void setDevData(const ElementType * aligned_data_row_major) + { + const cudaError err = cudaMemcpy2D( + data, + pitch, + aligned_data_row_major, + width*sizeof(ElementType), + width*sizeof(ElementType), + height, + cudaMemcpyHostToDevice); + if(err != cudaSuccess) + throw CudaException("Image: unable to copy data from host to device.", err); + } + + /// Download the data from the device memory to aligned_data_row_major, a preallocated array in host memory + __host__ + void getDevData(ElementType* aligned_data_row_major) const + { + const cudaError err = cudaMemcpy2D( + aligned_data_row_major, // destination memory address + width*sizeof(ElementType), // pitch of destination memory + data, // source memory address + pitch, // pitch of source memory + width*sizeof(ElementType), // width of matrix transfer (columns in bytes) + height, // height of matrix transfer + cudaMemcpyDeviceToHost); + if(err != cudaSuccess) + throw CudaException("Image: unable to copy data from device to host.", err); + } + + __host__ + ~DeviceImage() + { + cudaError err = cudaFree(data); + if(err != cudaSuccess) + throw CudaException("Image: unable to free allocated memory.", err); + err = cudaFree(dev_ptr); + if(err != cudaSuccess) + throw CudaException("Image: unable to free allocated memory.", err); + } + + __host__ + cudaChannelFormatDesc getCudaChannelFormatDesc() const + { + return cudaCreateChannelDesc(); + } + + __host__ + void zero() + { + const cudaError err = cudaMemset2D( + data, + pitch, + 0, + width*sizeof(ElementType), + height); + if(err != cudaSuccess) + throw CudaException("Image: unable to zero.", err); + } + + __host__ + DeviceImage & operator=(const DeviceImage &other_image) + { + if(this != &other_image) + { + assert(width == other_image.width && + height == other_image.height); + const cudaError err = cudaMemcpy2D(data, + pitch, + other_image.data, + other_image.pitch, + width*sizeof(ElementType), + height, + cudaMemcpyDeviceToDevice); + if(err != cudaSuccess) + throw CudaException("Image, operator '=': unable to copy data from another image.", err); + } + return *this; + } + + // fields + size_t width; + size_t height; + size_t pitch; + size_t stride; + ElementType * data; + DeviceImage *dev_ptr; +}; + +#endif // DEVICE_IMAGE_CUH diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/empty.cpp b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/empty.cpp new file mode 100644 index 0000000..6948223 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/empty.cpp @@ -0,0 +1 @@ +#include "empty.h" \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/empty.h b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/empty.h new file mode 100644 index 0000000..e69de29 diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/euroc.cpp b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/euroc.cpp new file mode 100644 index 0000000..307e255 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/euroc.cpp @@ -0,0 +1,378 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include "opencv2/highgui/highgui.hpp" +#include +#include + +#include "depth_render.cuh" + +using namespace cv; +using namespace std; +using namespace Eigen; + +typedef message_filters::sync_policies::ApproximateTime approx_policy; + +int *depth_hostptr; +cv::Mat depth_mat; + +int width, height; +cv::Mat cv_K, cv_D; +double fx,fy,cx,cy; +cv::Mat undist_map1, undist_map2; +bool is_distorted(false); + +DepthRender depthrender; +ros::Publisher pub_depth; +ros::Publisher pub_color; +ros::Publisher pub_posedimage; + +Matrix4d vicon2body; +Matrix4d cam02body; +Matrix4d cam2world; +Matrix4d vicon2leica; + +ros::Time receive_stamp; + +cv::Mat undistorted_image; + +struct PoseInfo +{ + Matrix4d pose; + double time; +}; + +vector gt_pose_vect; + +void render_currentpose(); + +bool read_pose(fstream &file) +{ + int count = 0; + bool good = true; + double para[16]; + while(good) + { + count ++; + for(int i = 0; i < 8 && good; i++) + { + good = good && file >> para[i]; + } + if(good) + { + Eigen::Vector3d request_position; + Eigen::Quaterniond request_pose; + request_position.x() = para[1]; + request_position.y() = para[2]; + request_position.z() = para[3]; + request_pose.w() = para[4]; + request_pose.x() = para[5]; + request_pose.y() = para[6]; + request_pose.z() = para[7]; + + PoseInfo info; + info.time = para[0]; + info.pose = Matrix4d::Identity(); + info.pose.block<3,3>(0,0) = request_pose.toRotationMatrix(); + info.pose(0,3) = para[1]; + info.pose(1,3) = para[2]; + info.pose(2,3) = para[3]; + gt_pose_vect.push_back(info); + } + } + printf("we have %d poses.\n", count); + return true; +} + +vector pts_3; +vector pts_2; +void imageBackFunc(int event, int x, int y, int flags, void* userdata) +{ + if( event == EVENT_LBUTTONDOWN ) + { + cout << "image is clicked - position (" << x << ", " << y << ")" << endl; + pts_2.push_back(cv::Point2f(x,y)); + } +} +void depthBackFunc(int event, int x, int y, int flags, void* userdata) +{ + if( event == EVENT_LBUTTONDOWN ) + { + cout << "depth is clicked - position (" << x << ", " << y << ")" << endl; + double depth = depth_mat.at(y,x); + double space_x = (x - cx) * depth / fx; + double space_y = (y - cy) * depth / fy; + double space_z = depth; + pts_3.push_back(cv::Point3f(space_x, space_y, space_z)); + } +} + +void solve_pnp() +{ +// translation : +// 0.994976 -0.0431638 0.0903361 -0.0338185 +// 0.0444475 0.998937 -0.012246 0.0541652 +// -0.0897114 0.0161996 0.995836 0.0384018 +// 0 0 0 1 + printf("we have %d pair points.\n", pts_3.size()); + if(pts_3.size() < 5 || pts_2.size() < 5) + { + return; + } + if(pts_3.size() != pts_2.size()) + { + printf("error, not equal!\n"); + return; + } + + cv::Mat r, rvec, t; + cv::solvePnP(pts_3, pts_2, cv_K, cv::Mat::zeros(4,1,CV_32FC1), rvec, t); + cv::Rodrigues(rvec, r); + Matrix3d R_ref; + for(int i=0;i<3;i++) + for(int j=0;j<3;j++) + { + R_ref(i,j) = r.at(i, j); + } + Matrix4d pnp_result = Matrix4d::Identity(); + pnp_result.block<3,3>(0,0) = R_ref; + pnp_result(0,3) = t.at(0, 0); + pnp_result(1,3) = t.at(1, 0); + pnp_result(2,3) = t.at(2, 0); + + vicon2leica = pnp_result.inverse(); + cout << "translation : " << endl << pnp_result << endl; +} + +void image_pose_callback( + const sensor_msgs::ImageConstPtr &image_input, + const geometry_msgs::TransformStampedConstPtr &pose_input) +{ + //time diff + double time_diff = fabs(image_input->header.stamp.toSec() - pose_input->header.stamp.toSec()) * 1000.0; + printf("time diff is %lf ms.\n", time_diff); + + //pose + Matrix4d Pose_receive = Matrix4d::Identity(); + + //using vicon + // Eigen::Vector3d request_position; + // Eigen::Quaterniond request_pose; + // request_position.x() = pose_input->transform.translation.x; + // request_position.y() = pose_input->transform.translation.y; + // request_position.z() = pose_input->transform.translation.z; + // request_pose.x() = pose_input->transform.rotation.x; + // request_pose.y() = pose_input->transform.rotation.y; + // request_pose.z() = pose_input->transform.rotation.z; + // request_pose.w() = pose_input->transform.rotation.w; + // Pose_receive.block<3,3>(0,0) = request_pose.toRotationMatrix(); + // Pose_receive(0,3) = request_position(0); + // Pose_receive(1,3) = request_position(1); + // Pose_receive(2,3) = request_position(2); + + //using ground truth + double image_time = image_input->header.stamp.toSec(); + double min_time_diff = 999.9; + int min_time_index = 0; + for(int i = 1; i < gt_pose_vect.size(); i++) + { + double time_diff = fabs(image_time - gt_pose_vect[i].time); + if(time_diff < min_time_diff) + { + min_time_diff = time_diff; + min_time_index = i; + } + } + printf("min time diff index %d, with diff time %lf ms.\n", min_time_index, min_time_diff*1000.0f); + Pose_receive = gt_pose_vect[min_time_index].pose; + + //convert to body pose + // Matrix4d body_pose = Pose_receive * vicon2body.inverse(); + Matrix4d body_pose = Pose_receive; + + //convert to cam pose + cam2world = body_pose * cam02body * vicon2leica; + + receive_stamp = pose_input->header.stamp; + + //image + cv_bridge::CvImageConstPtr cv_img_ptr = cv_bridge::toCvShare(image_input, sensor_msgs::image_encodings::MONO8); + cv::Mat img_8uC1 = cv_img_ptr->image; + undistorted_image.create(height, width, CV_8UC1); + if(is_distorted) + { + cv::remap(img_8uC1, undistorted_image, undist_map1, undist_map2, CV_INTER_LINEAR); + } + else + undistorted_image = img_8uC1; + + render_currentpose(); +} + +void render_currentpose() +{ + solve_pnp(); + + double this_time = ros::Time::now().toSec(); + + Matrix4d cam_pose = cam2world.inverse(); + + depthrender.render_pose(cam_pose, depth_hostptr); + + depth_mat = cv::Mat::zeros(height, width, CV_32FC1); + double min = 0.5; + double max = 1.0f; + for(int i = 0; i < height; i++) + for(int j = 0; j < width; j++) + { + float depth = (float)depth_hostptr[i * width + j] / 1000.0f; + depth = depth < 500.0f ? depth : 0; + max = depth > max ? depth : max; + depth_mat.at(i,j) = depth; + } + ROS_INFO("render cost %lf ms.", (ros::Time::now().toSec() - this_time) * 1000.0f); + printf("max_depth %lf.\n", max); + + cv_bridge::CvImage out_msg; + out_msg.header.stamp = receive_stamp; + out_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + out_msg.image = depth_mat.clone(); + pub_depth.publish(out_msg.toImageMsg()); + + cv::Mat adjMap; + depth_mat.convertTo(adjMap,CV_8UC1, 255 / (max-min), -min); + cv::Mat falseColorsMap; + cv::applyColorMap(adjMap, falseColorsMap, cv::COLORMAP_RAINBOW); + cv::Mat bgr_image; + cv::cvtColor(undistorted_image, bgr_image, cv::COLOR_GRAY2BGR); + cv::addWeighted(bgr_image, 0.2, falseColorsMap, 0.8, 0.0, falseColorsMap); + cv_bridge::CvImage cv_image_colored; + cv_image_colored.header.frame_id = "depthmap"; + cv_image_colored.encoding = sensor_msgs::image_encodings::BGR8; + cv_image_colored.image = falseColorsMap; + pub_color.publish(cv_image_colored.toImageMsg()); + + cv::imshow("bluefox_image", bgr_image); + cv::imshow("depth_image", adjMap); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "cloud_banchmark"); + ros::NodeHandle nh("~"); + + nh.getParam("cam_width", width); + nh.getParam("cam_height", height); + nh.getParam("cam_fx", fx); + nh.getParam("cam_fy", fy); + nh.getParam("cam_cx", cx); + nh.getParam("cam_cy", cy); + + depthrender.set_para(fx, fy, cx, cy, width, height); + + cv_K = (cv::Mat_(3, 3) << fx, 0.0f, cx, 0.0f, fy, cy, 0.0f, 0.0f, 1.0f); + if(nh.hasParam("cam_k1") && + nh.hasParam("cam_k2") && + nh.hasParam("cam_r1") && + nh.hasParam("cam_r2") ) + { + float k1, k2, r1, r2; + nh.getParam("cam_k1", k1); + nh.getParam("cam_k2", k2); + nh.getParam("cam_r1", r1); + nh.getParam("cam_r2", r2); + cv_D = (cv::Mat_(1, 4) << k1, k2, r1, r2); + cv::initUndistortRectifyMap( + cv_K, + cv_D, + cv::Mat_::eye(3,3), + cv_K, + cv::Size(width, height), + CV_16SC2, + undist_map1, undist_map2); + is_distorted = true; + } + if(is_distorted) + printf("need to rectify.\n"); + else + printf("do not need to rectify.\n"); + + vicon2body << 0.33638, -0.01749, 0.94156, 0.06901, + -0.02078, -0.99972, -0.01114, -0.02781, + 0.94150, -0.01582, -0.33665, -0.12395, + 0.0, 0.0, 0.0, 1.0; + cam02body << 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, + 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, + -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, + 0.0, 0.0, 0.0, 1.0; + cam2world = Matrix4d::Identity(); + + string cloud_path; + nh.getParam("cloud_path", cloud_path); + printf("cloud file %s\n", cloud_path.c_str()); + std::fstream data_file; + data_file.open(cloud_path.c_str(), ios::in); + vector cloud_data; + double x, y, z, i, r, g, b; + while(data_file >> x >> y >> z >> i >> r >> g >> b) + { + cloud_data.push_back(x); + cloud_data.push_back(y); + cloud_data.push_back(z); + } + data_file.close(); + printf("has points %d.\n", cloud_data.size() / 3 ); + + + string groundtruth_path = string("/home/denny/Downloads/wkx_bag/data.txt"); + std::fstream gt_file; + gt_file.open(groundtruth_path.c_str(), ios::in); + read_pose(gt_file); + gt_file.close(); + + //pass cloud_data to depth render + depthrender.set_data(cloud_data); + depth_hostptr = (int*) malloc(width * height * sizeof(int)); + + message_filters::Subscriber image_sub(nh, "/cam0/image_raw", 30); + message_filters::Subscriber pose_sub(nh, "/vicon/firefly_sbx/firefly_sbx", 30); + message_filters::Synchronizer sync2(approx_policy(100), image_sub, pose_sub); + sync2.registerCallback(boost::bind(image_pose_callback, _1, _2)); + + //publisher depth image and color image + pub_depth = nh.advertise("depth",1000); + pub_color = nh.advertise("colordepth",1000); + // pub_posedimage = nh.advertise("posedimage",1000); + + undistorted_image.create(height, width, CV_8UC1); + + cv::namedWindow("bluefox_image",1); + cv::namedWindow("depth_image",1); + setMouseCallback("bluefox_image", imageBackFunc, NULL); + setMouseCallback("depth_image", depthBackFunc, NULL); + vicon2leica = Matrix4d::Identity(); + + while(ros::ok()) + { + ros::spinOnce(); + cv::waitKey(30); + } +} \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/helper_math.h b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/helper_math.h new file mode 100644 index 0000000..c9c07c3 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/helper_math.h @@ -0,0 +1,1453 @@ +/** + * Copyright 1993-2013 NVIDIA Corporation. All rights reserved. + * + * Please refer to the NVIDIA end user license agreement (EULA) associated + * with this source code for terms and conditions that govern your use of + * this software. Any use, reproduction, disclosure, or distribution of + * this software and related documentation outside the terms of the EULA + * is strictly prohibited. + * + */ + +/* + * This file implements common mathematical operations on vector types + * (float3, float4 etc.) since these are not provided as standard by CUDA. + * + * The syntax is modeled on the Cg standard library. + * + * This is part of the Helper library includes + * + * Thanks to Linh Hah for additions and fixes. + */ + +#ifndef HELPER_MATH_H +#define HELPER_MATH_H + +#include "cuda_runtime.h" + +typedef unsigned int uint; +typedef unsigned short ushort; + +#ifndef EXIT_WAIVED +#define EXIT_WAIVED 2 +#endif + +#ifndef __CUDACC__ +#include + +//////////////////////////////////////////////////////////////////////////////// +// host implementations of CUDA functions +//////////////////////////////////////////////////////////////////////////////// + +inline float fminf(float a, float b) +{ + return a < b ? a : b; +} + +inline float fmaxf(float a, float b) +{ + return a > b ? a : b; +} + +inline int max(int a, int b) +{ + return a > b ? a : b; +} + +inline int min(int a, int b) +{ + return a < b ? a : b; +} + +inline float rsqrtf(float x) +{ + return 1.0f / sqrtf(x); +} +#endif + +//////////////////////////////////////////////////////////////////////////////// +// constructors +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 make_float2(float s) +{ + return make_float2(s, s); +} +inline __host__ __device__ float2 make_float2(float3 a) +{ + return make_float2(a.x, a.y); +} +inline __host__ __device__ float2 make_float2(int2 a) +{ + return make_float2(float(a.x), float(a.y)); +} +inline __host__ __device__ float2 make_float2(uint2 a) +{ + return make_float2(float(a.x), float(a.y)); +} + +inline __host__ __device__ int2 make_int2(int s) +{ + return make_int2(s, s); +} +inline __host__ __device__ int2 make_int2(int3 a) +{ + return make_int2(a.x, a.y); +} +inline __host__ __device__ int2 make_int2(uint2 a) +{ + return make_int2(int(a.x), int(a.y)); +} +inline __host__ __device__ int2 make_int2(float2 a) +{ + return make_int2(int(a.x), int(a.y)); +} + +inline __host__ __device__ uint2 make_uint2(uint s) +{ + return make_uint2(s, s); +} +inline __host__ __device__ uint2 make_uint2(uint3 a) +{ + return make_uint2(a.x, a.y); +} +inline __host__ __device__ uint2 make_uint2(int2 a) +{ + return make_uint2(uint(a.x), uint(a.y)); +} + +inline __host__ __device__ float3 make_float3(float s) +{ + return make_float3(s, s, s); +} +inline __host__ __device__ float3 make_float3(float2 a) +{ + return make_float3(a.x, a.y, 0.0f); +} +inline __host__ __device__ float3 make_float3(float2 a, float s) +{ + return make_float3(a.x, a.y, s); +} +inline __host__ __device__ float3 make_float3(float4 a) +{ + return make_float3(a.x, a.y, a.z); +} +inline __host__ __device__ float3 make_float3(int3 a) +{ + return make_float3(float(a.x), float(a.y), float(a.z)); +} +inline __host__ __device__ float3 make_float3(uint3 a) +{ + return make_float3(float(a.x), float(a.y), float(a.z)); +} + +inline __host__ __device__ int3 make_int3(int s) +{ + return make_int3(s, s, s); +} +inline __host__ __device__ int3 make_int3(int2 a) +{ + return make_int3(a.x, a.y, 0); +} +inline __host__ __device__ int3 make_int3(int2 a, int s) +{ + return make_int3(a.x, a.y, s); +} +inline __host__ __device__ int3 make_int3(uint3 a) +{ + return make_int3(int(a.x), int(a.y), int(a.z)); +} +inline __host__ __device__ int3 make_int3(float3 a) +{ + return make_int3(int(a.x), int(a.y), int(a.z)); +} + +inline __host__ __device__ uint3 make_uint3(uint s) +{ + return make_uint3(s, s, s); +} +inline __host__ __device__ uint3 make_uint3(uint2 a) +{ + return make_uint3(a.x, a.y, 0); +} +inline __host__ __device__ uint3 make_uint3(uint2 a, uint s) +{ + return make_uint3(a.x, a.y, s); +} +inline __host__ __device__ uint3 make_uint3(uint4 a) +{ + return make_uint3(a.x, a.y, a.z); +} +inline __host__ __device__ uint3 make_uint3(int3 a) +{ + return make_uint3(uint(a.x), uint(a.y), uint(a.z)); +} + +inline __host__ __device__ float4 make_float4(float s) +{ + return make_float4(s, s, s, s); +} +inline __host__ __device__ float4 make_float4(float3 a) +{ + return make_float4(a.x, a.y, a.z, 0.0f); +} +inline __host__ __device__ float4 make_float4(float3 a, float w) +{ + return make_float4(a.x, a.y, a.z, w); +} +inline __host__ __device__ float4 make_float4(int4 a) +{ + return make_float4(float(a.x), float(a.y), float(a.z), float(a.w)); +} +inline __host__ __device__ float4 make_float4(uint4 a) +{ + return make_float4(float(a.x), float(a.y), float(a.z), float(a.w)); +} + +inline __host__ __device__ int4 make_int4(int s) +{ + return make_int4(s, s, s, s); +} +inline __host__ __device__ int4 make_int4(int3 a) +{ + return make_int4(a.x, a.y, a.z, 0); +} +inline __host__ __device__ int4 make_int4(int3 a, int w) +{ + return make_int4(a.x, a.y, a.z, w); +} +inline __host__ __device__ int4 make_int4(uint4 a) +{ + return make_int4(int(a.x), int(a.y), int(a.z), int(a.w)); +} +inline __host__ __device__ int4 make_int4(float4 a) +{ + return make_int4(int(a.x), int(a.y), int(a.z), int(a.w)); +} + + +inline __host__ __device__ uint4 make_uint4(uint s) +{ + return make_uint4(s, s, s, s); +} +inline __host__ __device__ uint4 make_uint4(uint3 a) +{ + return make_uint4(a.x, a.y, a.z, 0); +} +inline __host__ __device__ uint4 make_uint4(uint3 a, uint w) +{ + return make_uint4(a.x, a.y, a.z, w); +} +inline __host__ __device__ uint4 make_uint4(int4 a) +{ + return make_uint4(uint(a.x), uint(a.y), uint(a.z), uint(a.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// negate +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 operator-(float2 &a) +{ + return make_float2(-a.x, -a.y); +} +inline __host__ __device__ int2 operator-(int2 &a) +{ + return make_int2(-a.x, -a.y); +} +inline __host__ __device__ float3 operator-(float3 &a) +{ + return make_float3(-a.x, -a.y, -a.z); +} +inline __host__ __device__ int3 operator-(int3 &a) +{ + return make_int3(-a.x, -a.y, -a.z); +} +inline __host__ __device__ float4 operator-(float4 &a) +{ + return make_float4(-a.x, -a.y, -a.z, -a.w); +} +inline __host__ __device__ int4 operator-(int4 &a) +{ + return make_int4(-a.x, -a.y, -a.z, -a.w); +} + +//////////////////////////////////////////////////////////////////////////////// +// addition +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 operator+(float2 a, float2 b) +{ + return make_float2(a.x + b.x, a.y + b.y); +} +inline __host__ __device__ void operator+=(float2 &a, float2 b) +{ + a.x += b.x; + a.y += b.y; +} +inline __host__ __device__ float2 operator+(float2 a, float b) +{ + return make_float2(a.x + b, a.y + b); +} +inline __host__ __device__ float2 operator+(float b, float2 a) +{ + return make_float2(a.x + b, a.y + b); +} +inline __host__ __device__ void operator+=(float2 &a, float b) +{ + a.x += b; + a.y += b; +} + +inline __host__ __device__ int2 operator+(int2 a, int2 b) +{ + return make_int2(a.x + b.x, a.y + b.y); +} +inline __host__ __device__ void operator+=(int2 &a, int2 b) +{ + a.x += b.x; + a.y += b.y; +} +inline __host__ __device__ int2 operator+(int2 a, int b) +{ + return make_int2(a.x + b, a.y + b); +} +inline __host__ __device__ int2 operator+(int b, int2 a) +{ + return make_int2(a.x + b, a.y + b); +} +inline __host__ __device__ void operator+=(int2 &a, int b) +{ + a.x += b; + a.y += b; +} + +inline __host__ __device__ uint2 operator+(uint2 a, uint2 b) +{ + return make_uint2(a.x + b.x, a.y + b.y); +} +inline __host__ __device__ void operator+=(uint2 &a, uint2 b) +{ + a.x += b.x; + a.y += b.y; +} +inline __host__ __device__ uint2 operator+(uint2 a, uint b) +{ + return make_uint2(a.x + b, a.y + b); +} +inline __host__ __device__ uint2 operator+(uint b, uint2 a) +{ + return make_uint2(a.x + b, a.y + b); +} +inline __host__ __device__ void operator+=(uint2 &a, uint b) +{ + a.x += b; + a.y += b; +} + + +inline __host__ __device__ float3 operator+(float3 a, float3 b) +{ + return make_float3(a.x + b.x, a.y + b.y, a.z + b.z); +} +inline __host__ __device__ void operator+=(float3 &a, float3 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; +} +inline __host__ __device__ float3 operator+(float3 a, float b) +{ + return make_float3(a.x + b, a.y + b, a.z + b); +} +inline __host__ __device__ void operator+=(float3 &a, float b) +{ + a.x += b; + a.y += b; + a.z += b; +} + +inline __host__ __device__ int3 operator+(int3 a, int3 b) +{ + return make_int3(a.x + b.x, a.y + b.y, a.z + b.z); +} +inline __host__ __device__ void operator+=(int3 &a, int3 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; +} +inline __host__ __device__ int3 operator+(int3 a, int b) +{ + return make_int3(a.x + b, a.y + b, a.z + b); +} +inline __host__ __device__ void operator+=(int3 &a, int b) +{ + a.x += b; + a.y += b; + a.z += b; +} + +inline __host__ __device__ uint3 operator+(uint3 a, uint3 b) +{ + return make_uint3(a.x + b.x, a.y + b.y, a.z + b.z); +} +inline __host__ __device__ void operator+=(uint3 &a, uint3 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; +} +inline __host__ __device__ uint3 operator+(uint3 a, uint b) +{ + return make_uint3(a.x + b, a.y + b, a.z + b); +} +inline __host__ __device__ void operator+=(uint3 &a, uint b) +{ + a.x += b; + a.y += b; + a.z += b; +} + +inline __host__ __device__ int3 operator+(int b, int3 a) +{ + return make_int3(a.x + b, a.y + b, a.z + b); +} +inline __host__ __device__ uint3 operator+(uint b, uint3 a) +{ + return make_uint3(a.x + b, a.y + b, a.z + b); +} +inline __host__ __device__ float3 operator+(float b, float3 a) +{ + return make_float3(a.x + b, a.y + b, a.z + b); +} + +inline __host__ __device__ float4 operator+(float4 a, float4 b) +{ + return make_float4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); +} +inline __host__ __device__ void operator+=(float4 &a, float4 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; + a.w += b.w; +} +inline __host__ __device__ float4 operator+(float4 a, float b) +{ + return make_float4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ float4 operator+(float b, float4 a) +{ + return make_float4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ void operator+=(float4 &a, float b) +{ + a.x += b; + a.y += b; + a.z += b; + a.w += b; +} + +inline __host__ __device__ int4 operator+(int4 a, int4 b) +{ + return make_int4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); +} +inline __host__ __device__ void operator+=(int4 &a, int4 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; + a.w += b.w; +} +inline __host__ __device__ int4 operator+(int4 a, int b) +{ + return make_int4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ int4 operator+(int b, int4 a) +{ + return make_int4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ void operator+=(int4 &a, int b) +{ + a.x += b; + a.y += b; + a.z += b; + a.w += b; +} + +inline __host__ __device__ uint4 operator+(uint4 a, uint4 b) +{ + return make_uint4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); +} +inline __host__ __device__ void operator+=(uint4 &a, uint4 b) +{ + a.x += b.x; + a.y += b.y; + a.z += b.z; + a.w += b.w; +} +inline __host__ __device__ uint4 operator+(uint4 a, uint b) +{ + return make_uint4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ uint4 operator+(uint b, uint4 a) +{ + return make_uint4(a.x + b, a.y + b, a.z + b, a.w + b); +} +inline __host__ __device__ void operator+=(uint4 &a, uint b) +{ + a.x += b; + a.y += b; + a.z += b; + a.w += b; +} + +//////////////////////////////////////////////////////////////////////////////// +// subtract +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 operator-(float2 a, float2 b) +{ + return make_float2(a.x - b.x, a.y - b.y); +} +inline __host__ __device__ void operator-=(float2 &a, float2 b) +{ + a.x -= b.x; + a.y -= b.y; +} +inline __host__ __device__ float2 operator-(float2 a, float b) +{ + return make_float2(a.x - b, a.y - b); +} +inline __host__ __device__ float2 operator-(float b, float2 a) +{ + return make_float2(b - a.x, b - a.y); +} +inline __host__ __device__ void operator-=(float2 &a, float b) +{ + a.x -= b; + a.y -= b; +} + +inline __host__ __device__ int2 operator-(int2 a, int2 b) +{ + return make_int2(a.x - b.x, a.y - b.y); +} +inline __host__ __device__ void operator-=(int2 &a, int2 b) +{ + a.x -= b.x; + a.y -= b.y; +} +inline __host__ __device__ int2 operator-(int2 a, int b) +{ + return make_int2(a.x - b, a.y - b); +} +inline __host__ __device__ int2 operator-(int b, int2 a) +{ + return make_int2(b - a.x, b - a.y); +} +inline __host__ __device__ void operator-=(int2 &a, int b) +{ + a.x -= b; + a.y -= b; +} + +inline __host__ __device__ uint2 operator-(uint2 a, uint2 b) +{ + return make_uint2(a.x - b.x, a.y - b.y); +} +inline __host__ __device__ void operator-=(uint2 &a, uint2 b) +{ + a.x -= b.x; + a.y -= b.y; +} +inline __host__ __device__ uint2 operator-(uint2 a, uint b) +{ + return make_uint2(a.x - b, a.y - b); +} +inline __host__ __device__ uint2 operator-(uint b, uint2 a) +{ + return make_uint2(b - a.x, b - a.y); +} +inline __host__ __device__ void operator-=(uint2 &a, uint b) +{ + a.x -= b; + a.y -= b; +} + +inline __host__ __device__ float3 operator-(float3 a, float3 b) +{ + return make_float3(a.x - b.x, a.y - b.y, a.z - b.z); +} +inline __host__ __device__ void operator-=(float3 &a, float3 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; +} +inline __host__ __device__ float3 operator-(float3 a, float b) +{ + return make_float3(a.x - b, a.y - b, a.z - b); +} +inline __host__ __device__ float3 operator-(float b, float3 a) +{ + return make_float3(b - a.x, b - a.y, b - a.z); +} +inline __host__ __device__ void operator-=(float3 &a, float b) +{ + a.x -= b; + a.y -= b; + a.z -= b; +} + +inline __host__ __device__ int3 operator-(int3 a, int3 b) +{ + return make_int3(a.x - b.x, a.y - b.y, a.z - b.z); +} +inline __host__ __device__ void operator-=(int3 &a, int3 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; +} +inline __host__ __device__ int3 operator-(int3 a, int b) +{ + return make_int3(a.x - b, a.y - b, a.z - b); +} +inline __host__ __device__ int3 operator-(int b, int3 a) +{ + return make_int3(b - a.x, b - a.y, b - a.z); +} +inline __host__ __device__ void operator-=(int3 &a, int b) +{ + a.x -= b; + a.y -= b; + a.z -= b; +} + +inline __host__ __device__ uint3 operator-(uint3 a, uint3 b) +{ + return make_uint3(a.x - b.x, a.y - b.y, a.z - b.z); +} +inline __host__ __device__ void operator-=(uint3 &a, uint3 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; +} +inline __host__ __device__ uint3 operator-(uint3 a, uint b) +{ + return make_uint3(a.x - b, a.y - b, a.z - b); +} +inline __host__ __device__ uint3 operator-(uint b, uint3 a) +{ + return make_uint3(b - a.x, b - a.y, b - a.z); +} +inline __host__ __device__ void operator-=(uint3 &a, uint b) +{ + a.x -= b; + a.y -= b; + a.z -= b; +} + +inline __host__ __device__ float4 operator-(float4 a, float4 b) +{ + return make_float4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); +} +inline __host__ __device__ void operator-=(float4 &a, float4 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; + a.w -= b.w; +} +inline __host__ __device__ float4 operator-(float4 a, float b) +{ + return make_float4(a.x - b, a.y - b, a.z - b, a.w - b); +} +inline __host__ __device__ void operator-=(float4 &a, float b) +{ + a.x -= b; + a.y -= b; + a.z -= b; + a.w -= b; +} + +inline __host__ __device__ int4 operator-(int4 a, int4 b) +{ + return make_int4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); +} +inline __host__ __device__ void operator-=(int4 &a, int4 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; + a.w -= b.w; +} +inline __host__ __device__ int4 operator-(int4 a, int b) +{ + return make_int4(a.x - b, a.y - b, a.z - b, a.w - b); +} +inline __host__ __device__ int4 operator-(int b, int4 a) +{ + return make_int4(b - a.x, b - a.y, b - a.z, b - a.w); +} +inline __host__ __device__ void operator-=(int4 &a, int b) +{ + a.x -= b; + a.y -= b; + a.z -= b; + a.w -= b; +} + +inline __host__ __device__ uint4 operator-(uint4 a, uint4 b) +{ + return make_uint4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); +} +inline __host__ __device__ void operator-=(uint4 &a, uint4 b) +{ + a.x -= b.x; + a.y -= b.y; + a.z -= b.z; + a.w -= b.w; +} +inline __host__ __device__ uint4 operator-(uint4 a, uint b) +{ + return make_uint4(a.x - b, a.y - b, a.z - b, a.w - b); +} +inline __host__ __device__ uint4 operator-(uint b, uint4 a) +{ + return make_uint4(b - a.x, b - a.y, b - a.z, b - a.w); +} +inline __host__ __device__ void operator-=(uint4 &a, uint b) +{ + a.x -= b; + a.y -= b; + a.z -= b; + a.w -= b; +} + +//////////////////////////////////////////////////////////////////////////////// +// multiply +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 operator*(float2 a, float2 b) +{ + return make_float2(a.x * b.x, a.y * b.y); +} +inline __host__ __device__ void operator*=(float2 &a, float2 b) +{ + a.x *= b.x; + a.y *= b.y; +} +inline __host__ __device__ float2 operator*(float2 a, float b) +{ + return make_float2(a.x * b, a.y * b); +} +inline __host__ __device__ float2 operator*(float b, float2 a) +{ + return make_float2(b * a.x, b * a.y); +} +inline __host__ __device__ void operator*=(float2 &a, float b) +{ + a.x *= b; + a.y *= b; +} + +inline __host__ __device__ int2 operator*(int2 a, int2 b) +{ + return make_int2(a.x * b.x, a.y * b.y); +} +inline __host__ __device__ void operator*=(int2 &a, int2 b) +{ + a.x *= b.x; + a.y *= b.y; +} +inline __host__ __device__ int2 operator*(int2 a, int b) +{ + return make_int2(a.x * b, a.y * b); +} +inline __host__ __device__ int2 operator*(int b, int2 a) +{ + return make_int2(b * a.x, b * a.y); +} +inline __host__ __device__ void operator*=(int2 &a, int b) +{ + a.x *= b; + a.y *= b; +} + +inline __host__ __device__ uint2 operator*(uint2 a, uint2 b) +{ + return make_uint2(a.x * b.x, a.y * b.y); +} +inline __host__ __device__ void operator*=(uint2 &a, uint2 b) +{ + a.x *= b.x; + a.y *= b.y; +} +inline __host__ __device__ uint2 operator*(uint2 a, uint b) +{ + return make_uint2(a.x * b, a.y * b); +} +inline __host__ __device__ uint2 operator*(uint b, uint2 a) +{ + return make_uint2(b * a.x, b * a.y); +} +inline __host__ __device__ void operator*=(uint2 &a, uint b) +{ + a.x *= b; + a.y *= b; +} + +inline __host__ __device__ float3 operator*(float3 a, float3 b) +{ + return make_float3(a.x * b.x, a.y * b.y, a.z * b.z); +} +inline __host__ __device__ void operator*=(float3 &a, float3 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; +} +inline __host__ __device__ float3 operator*(float3 a, float b) +{ + return make_float3(a.x * b, a.y * b, a.z * b); +} +inline __host__ __device__ float3 operator*(float b, float3 a) +{ + return make_float3(b * a.x, b * a.y, b * a.z); +} +inline __host__ __device__ void operator*=(float3 &a, float b) +{ + a.x *= b; + a.y *= b; + a.z *= b; +} + +inline __host__ __device__ int3 operator*(int3 a, int3 b) +{ + return make_int3(a.x * b.x, a.y * b.y, a.z * b.z); +} +inline __host__ __device__ void operator*=(int3 &a, int3 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; +} +inline __host__ __device__ int3 operator*(int3 a, int b) +{ + return make_int3(a.x * b, a.y * b, a.z * b); +} +inline __host__ __device__ int3 operator*(int b, int3 a) +{ + return make_int3(b * a.x, b * a.y, b * a.z); +} +inline __host__ __device__ void operator*=(int3 &a, int b) +{ + a.x *= b; + a.y *= b; + a.z *= b; +} + +inline __host__ __device__ uint3 operator*(uint3 a, uint3 b) +{ + return make_uint3(a.x * b.x, a.y * b.y, a.z * b.z); +} +inline __host__ __device__ void operator*=(uint3 &a, uint3 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; +} +inline __host__ __device__ uint3 operator*(uint3 a, uint b) +{ + return make_uint3(a.x * b, a.y * b, a.z * b); +} +inline __host__ __device__ uint3 operator*(uint b, uint3 a) +{ + return make_uint3(b * a.x, b * a.y, b * a.z); +} +inline __host__ __device__ void operator*=(uint3 &a, uint b) +{ + a.x *= b; + a.y *= b; + a.z *= b; +} + +inline __host__ __device__ float4 operator*(float4 a, float4 b) +{ + return make_float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w); +} +inline __host__ __device__ void operator*=(float4 &a, float4 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; + a.w *= b.w; +} +inline __host__ __device__ float4 operator*(float4 a, float b) +{ + return make_float4(a.x * b, a.y * b, a.z * b, a.w * b); +} +inline __host__ __device__ float4 operator*(float b, float4 a) +{ + return make_float4(b * a.x, b * a.y, b * a.z, b * a.w); +} +inline __host__ __device__ void operator*=(float4 &a, float b) +{ + a.x *= b; + a.y *= b; + a.z *= b; + a.w *= b; +} + +inline __host__ __device__ int4 operator*(int4 a, int4 b) +{ + return make_int4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w); +} +inline __host__ __device__ void operator*=(int4 &a, int4 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; + a.w *= b.w; +} +inline __host__ __device__ int4 operator*(int4 a, int b) +{ + return make_int4(a.x * b, a.y * b, a.z * b, a.w * b); +} +inline __host__ __device__ int4 operator*(int b, int4 a) +{ + return make_int4(b * a.x, b * a.y, b * a.z, b * a.w); +} +inline __host__ __device__ void operator*=(int4 &a, int b) +{ + a.x *= b; + a.y *= b; + a.z *= b; + a.w *= b; +} + +inline __host__ __device__ uint4 operator*(uint4 a, uint4 b) +{ + return make_uint4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w); +} +inline __host__ __device__ void operator*=(uint4 &a, uint4 b) +{ + a.x *= b.x; + a.y *= b.y; + a.z *= b.z; + a.w *= b.w; +} +inline __host__ __device__ uint4 operator*(uint4 a, uint b) +{ + return make_uint4(a.x * b, a.y * b, a.z * b, a.w * b); +} +inline __host__ __device__ uint4 operator*(uint b, uint4 a) +{ + return make_uint4(b * a.x, b * a.y, b * a.z, b * a.w); +} +inline __host__ __device__ void operator*=(uint4 &a, uint b) +{ + a.x *= b; + a.y *= b; + a.z *= b; + a.w *= b; +} + +//////////////////////////////////////////////////////////////////////////////// +// divide +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 operator/(float2 a, float2 b) +{ + return make_float2(a.x / b.x, a.y / b.y); +} +inline __host__ __device__ void operator/=(float2 &a, float2 b) +{ + a.x /= b.x; + a.y /= b.y; +} +inline __host__ __device__ float2 operator/(float2 a, float b) +{ + return make_float2(a.x / b, a.y / b); +} +inline __host__ __device__ void operator/=(float2 &a, float b) +{ + a.x /= b; + a.y /= b; +} +inline __host__ __device__ float2 operator/(float b, float2 a) +{ + return make_float2(b / a.x, b / a.y); +} + +inline __host__ __device__ float3 operator/(float3 a, float3 b) +{ + return make_float3(a.x / b.x, a.y / b.y, a.z / b.z); +} +inline __host__ __device__ void operator/=(float3 &a, float3 b) +{ + a.x /= b.x; + a.y /= b.y; + a.z /= b.z; +} +inline __host__ __device__ float3 operator/(float3 a, float b) +{ + return make_float3(a.x / b, a.y / b, a.z / b); +} +inline __host__ __device__ void operator/=(float3 &a, float b) +{ + a.x /= b; + a.y /= b; + a.z /= b; +} +inline __host__ __device__ float3 operator/(float b, float3 a) +{ + return make_float3(b / a.x, b / a.y, b / a.z); +} + +inline __host__ __device__ float4 operator/(float4 a, float4 b) +{ + return make_float4(a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w); +} +inline __host__ __device__ void operator/=(float4 &a, float4 b) +{ + a.x /= b.x; + a.y /= b.y; + a.z /= b.z; + a.w /= b.w; +} +inline __host__ __device__ float4 operator/(float4 a, float b) +{ + return make_float4(a.x / b, a.y / b, a.z / b, a.w / b); +} +inline __host__ __device__ void operator/=(float4 &a, float b) +{ + a.x /= b; + a.y /= b; + a.z /= b; + a.w /= b; +} +inline __host__ __device__ float4 operator/(float b, float4 a) +{ + return make_float4(b / a.x, b / a.y, b / a.z, b / a.w); +} + +//////////////////////////////////////////////////////////////////////////////// +// min +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 fminf(float2 a, float2 b) +{ + return make_float2(fminf(a.x,b.x), fminf(a.y,b.y)); +} +inline __host__ __device__ float3 fminf(float3 a, float3 b) +{ + return make_float3(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z)); +} +inline __host__ __device__ float4 fminf(float4 a, float4 b) +{ + return make_float4(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z), fminf(a.w,b.w)); +} + +inline __host__ __device__ int2 min(int2 a, int2 b) +{ + return make_int2(min(a.x,b.x), min(a.y,b.y)); +} +inline __host__ __device__ int3 min(int3 a, int3 b) +{ + return make_int3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z)); +} +inline __host__ __device__ int4 min(int4 a, int4 b) +{ + return make_int4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w)); +} + +inline __host__ __device__ uint2 min(uint2 a, uint2 b) +{ + return make_uint2(min(a.x,b.x), min(a.y,b.y)); +} +inline __host__ __device__ uint3 min(uint3 a, uint3 b) +{ + return make_uint3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z)); +} +inline __host__ __device__ uint4 min(uint4 a, uint4 b) +{ + return make_uint4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// max +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 fmaxf(float2 a, float2 b) +{ + return make_float2(fmaxf(a.x,b.x), fmaxf(a.y,b.y)); +} +inline __host__ __device__ float3 fmaxf(float3 a, float3 b) +{ + return make_float3(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z)); +} +inline __host__ __device__ float4 fmaxf(float4 a, float4 b) +{ + return make_float4(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z), fmaxf(a.w,b.w)); +} + +inline __host__ __device__ int2 max(int2 a, int2 b) +{ + return make_int2(max(a.x,b.x), max(a.y,b.y)); +} +inline __host__ __device__ int3 max(int3 a, int3 b) +{ + return make_int3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z)); +} +inline __host__ __device__ int4 max(int4 a, int4 b) +{ + return make_int4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w)); +} + +inline __host__ __device__ uint2 max(uint2 a, uint2 b) +{ + return make_uint2(max(a.x,b.x), max(a.y,b.y)); +} +inline __host__ __device__ uint3 max(uint3 a, uint3 b) +{ + return make_uint3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z)); +} +inline __host__ __device__ uint4 max(uint4 a, uint4 b) +{ + return make_uint4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// lerp +// - linear interpolation between a and b, based on value t in [0, 1] range +//////////////////////////////////////////////////////////////////////////////// + +inline __device__ __host__ float lerp(float a, float b, float t) +{ + return a + t*(b-a); +} +inline __device__ __host__ float2 lerp(float2 a, float2 b, float t) +{ + return a + t*(b-a); +} +inline __device__ __host__ float3 lerp(float3 a, float3 b, float t) +{ + return a + t*(b-a); +} +inline __device__ __host__ float4 lerp(float4 a, float4 b, float t) +{ + return a + t*(b-a); +} + +//////////////////////////////////////////////////////////////////////////////// +// clamp +// - clamp the value v to be in the range [a, b] +//////////////////////////////////////////////////////////////////////////////// + +inline __device__ __host__ float clamp(float f, float a, float b) +{ + return fmaxf(a, fminf(f, b)); +} +inline __device__ __host__ int clamp(int f, int a, int b) +{ + return max(a, min(f, b)); +} +inline __device__ __host__ uint clamp(uint f, uint a, uint b) +{ + return max(a, min(f, b)); +} + +inline __device__ __host__ float2 clamp(float2 v, float a, float b) +{ + return make_float2(clamp(v.x, a, b), clamp(v.y, a, b)); +} +inline __device__ __host__ float2 clamp(float2 v, float2 a, float2 b) +{ + return make_float2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y)); +} +inline __device__ __host__ float3 clamp(float3 v, float a, float b) +{ + return make_float3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b)); +} +inline __device__ __host__ float3 clamp(float3 v, float3 a, float3 b) +{ + return make_float3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z)); +} +inline __device__ __host__ float4 clamp(float4 v, float a, float b) +{ + return make_float4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b)); +} +inline __device__ __host__ float4 clamp(float4 v, float4 a, float4 b) +{ + return make_float4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w)); +} + +inline __device__ __host__ int2 clamp(int2 v, int a, int b) +{ + return make_int2(clamp(v.x, a, b), clamp(v.y, a, b)); +} +inline __device__ __host__ int2 clamp(int2 v, int2 a, int2 b) +{ + return make_int2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y)); +} +inline __device__ __host__ int3 clamp(int3 v, int a, int b) +{ + return make_int3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b)); +} +inline __device__ __host__ int3 clamp(int3 v, int3 a, int3 b) +{ + return make_int3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z)); +} +inline __device__ __host__ int4 clamp(int4 v, int a, int b) +{ + return make_int4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b)); +} +inline __device__ __host__ int4 clamp(int4 v, int4 a, int4 b) +{ + return make_int4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w)); +} + +inline __device__ __host__ uint2 clamp(uint2 v, uint a, uint b) +{ + return make_uint2(clamp(v.x, a, b), clamp(v.y, a, b)); +} +inline __device__ __host__ uint2 clamp(uint2 v, uint2 a, uint2 b) +{ + return make_uint2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y)); +} +inline __device__ __host__ uint3 clamp(uint3 v, uint a, uint b) +{ + return make_uint3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b)); +} +inline __device__ __host__ uint3 clamp(uint3 v, uint3 a, uint3 b) +{ + return make_uint3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z)); +} +inline __device__ __host__ uint4 clamp(uint4 v, uint a, uint b) +{ + return make_uint4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b)); +} +inline __device__ __host__ uint4 clamp(uint4 v, uint4 a, uint4 b) +{ + return make_uint4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// dot product +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float dot(float2 a, float2 b) +{ + return a.x * b.x + a.y * b.y; +} +inline __host__ __device__ float dot(float3 a, float3 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} +inline __host__ __device__ float dot(float4 a, float4 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; +} + +inline __host__ __device__ int dot(int2 a, int2 b) +{ + return a.x * b.x + a.y * b.y; +} +inline __host__ __device__ int dot(int3 a, int3 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} +inline __host__ __device__ int dot(int4 a, int4 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; +} + +inline __host__ __device__ uint dot(uint2 a, uint2 b) +{ + return a.x * b.x + a.y * b.y; +} +inline __host__ __device__ uint dot(uint3 a, uint3 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} +inline __host__ __device__ uint dot(uint4 a, uint4 b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; +} + +//////////////////////////////////////////////////////////////////////////////// +// length +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float length(float2 v) +{ + return sqrtf(dot(v, v)); +} +inline __host__ __device__ float length(float3 v) +{ + return sqrtf(dot(v, v)); +} +inline __host__ __device__ float length(float4 v) +{ + return sqrtf(dot(v, v)); +} + +//////////////////////////////////////////////////////////////////////////////// +// normalize +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 normalize(float2 v) +{ + float invLen = rsqrtf(dot(v, v)); + return v * invLen; +} +inline __host__ __device__ float3 normalize(float3 v) +{ + float invLen = rsqrtf(dot(v, v)); + return v * invLen; +} +inline __host__ __device__ float4 normalize(float4 v) +{ + float invLen = rsqrtf(dot(v, v)); + return v * invLen; +} + +//////////////////////////////////////////////////////////////////////////////// +// floor +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 floorf(float2 v) +{ + return make_float2(floorf(v.x), floorf(v.y)); +} +inline __host__ __device__ float3 floorf(float3 v) +{ + return make_float3(floorf(v.x), floorf(v.y), floorf(v.z)); +} +inline __host__ __device__ float4 floorf(float4 v) +{ + return make_float4(floorf(v.x), floorf(v.y), floorf(v.z), floorf(v.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// frac - returns the fractional portion of a scalar or each vector component +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float fracf(float v) +{ + return v - floorf(v); +} +inline __host__ __device__ float2 fracf(float2 v) +{ + return make_float2(fracf(v.x), fracf(v.y)); +} +inline __host__ __device__ float3 fracf(float3 v) +{ + return make_float3(fracf(v.x), fracf(v.y), fracf(v.z)); +} +inline __host__ __device__ float4 fracf(float4 v) +{ + return make_float4(fracf(v.x), fracf(v.y), fracf(v.z), fracf(v.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// fmod +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 fmodf(float2 a, float2 b) +{ + return make_float2(fmodf(a.x, b.x), fmodf(a.y, b.y)); +} +inline __host__ __device__ float3 fmodf(float3 a, float3 b) +{ + return make_float3(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z)); +} +inline __host__ __device__ float4 fmodf(float4 a, float4 b) +{ + return make_float4(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z), fmodf(a.w, b.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// absolute value +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float2 fabs(float2 v) +{ + return make_float2(fabs(v.x), fabs(v.y)); +} +inline __host__ __device__ float3 fabs(float3 v) +{ + return make_float3(fabs(v.x), fabs(v.y), fabs(v.z)); +} +inline __host__ __device__ float4 fabs(float4 v) +{ + return make_float4(fabs(v.x), fabs(v.y), fabs(v.z), fabs(v.w)); +} + +inline __host__ __device__ int2 abs(int2 v) +{ + return make_int2(abs(v.x), abs(v.y)); +} +inline __host__ __device__ int3 abs(int3 v) +{ + return make_int3(abs(v.x), abs(v.y), abs(v.z)); +} +inline __host__ __device__ int4 abs(int4 v) +{ + return make_int4(abs(v.x), abs(v.y), abs(v.z), abs(v.w)); +} + +//////////////////////////////////////////////////////////////////////////////// +// reflect +// - returns reflection of incident ray I around surface normal N +// - N should be normalized, reflected vector's length is equal to length of I +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float3 reflect(float3 i, float3 n) +{ + return i - 2.0f * n * dot(n,i); +} + +//////////////////////////////////////////////////////////////////////////////// +// cross product +//////////////////////////////////////////////////////////////////////////////// + +inline __host__ __device__ float3 cross(float3 a, float3 b) +{ + return make_float3(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x); +} + +//////////////////////////////////////////////////////////////////////////////// +// smoothstep +// - returns 0 if x < a +// - returns 1 if x > b +// - otherwise returns smooth interpolation between 0 and 1 based on x +//////////////////////////////////////////////////////////////////////////////// + +inline __device__ __host__ float smoothstep(float a, float b, float x) +{ + float y = clamp((x - a) / (b - a), 0.0f, 1.0f); + return (y*y*(3.0f - (2.0f*y))); +} +inline __device__ __host__ float2 smoothstep(float2 a, float2 b, float2 x) +{ + float2 y = clamp((x - a) / (b - a), 0.0f, 1.0f); + return (y*y*(make_float2(3.0f) - (make_float2(2.0f)*y))); +} +inline __device__ __host__ float3 smoothstep(float3 a, float3 b, float3 x) +{ + float3 y = clamp((x - a) / (b - a), 0.0f, 1.0f); + return (y*y*(make_float3(3.0f) - (make_float3(2.0f)*y))); +} +inline __device__ __host__ float4 smoothstep(float4 a, float4 b, float4 x) +{ + float4 y = clamp((x - a) / (b - a), 0.0f, 1.0f); + return (y*y*(make_float4(3.0f) - (make_float4(2.0f)*y))); +} + +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/pcl_render_node.cpp b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/pcl_render_node.cpp new file mode 100644 index 0000000..4e1c496 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/pcl_render_node.cpp @@ -0,0 +1,390 @@ +#include +#include +#include +//include ros dep. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tf/tf.h" +#include "tf/transform_datatypes.h" +#include +//include pcl dep +#include +#include +#include +#include + #include +//include opencv and eigen +#include +#include "opencv2/highgui/highgui.hpp" +#include +#include +#include + +//#include +#include "depth_render.cuh" +#include "quadrotor_msgs/PositionCommand.h" +using namespace cv; +using namespace std; +using namespace Eigen; + +int *depth_hostptr; +cv::Mat depth_mat; + +//camera param +int width, height; +double fx,fy,cx,cy; + +DepthRender depthrender; +ros::Publisher pub_depth; +ros::Publisher pub_color; +ros::Publisher pub_pose; +ros::Publisher pub_pcl_wolrd; + +sensor_msgs::PointCloud2 local_map_pcl; +sensor_msgs::PointCloud2 local_depth_pcl; + +ros::Subscriber odom_sub; +ros::Subscriber global_map_sub, local_map_sub; + +ros::Timer local_sensing_timer, estimation_timer; + +bool has_global_map(false); +bool has_local_map(false); +bool has_odom(false); + +Matrix4d cam02body; +Matrix4d cam2world; +Eigen::Quaterniond cam2world_quat; +nav_msgs::Odometry _odom; + +double sensing_horizon, sensing_rate, estimation_rate; +double _x_size, _y_size, _z_size; +double _gl_xl, _gl_yl, _gl_zl; +double _resolution, _inv_resolution; +int _GLX_SIZE, _GLY_SIZE, _GLZ_SIZE; + +ros::Time last_odom_stamp = ros::TIME_MAX; +Eigen::Vector3d last_pose_world; + +void render_currentpose(); +void render_pcl_world(); + +inline Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i & index) +{ + Eigen::Vector3d pt; + pt(0) = ((double)index(0) + 0.5) * _resolution + _gl_xl; + pt(1) = ((double)index(1) + 0.5) * _resolution + _gl_yl; + pt(2) = ((double)index(2) + 0.5) * _resolution + _gl_zl; + + return pt; +}; + +inline Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d & pt) +{ + Eigen::Vector3i idx; + idx(0) = std::min( std::max( int( (pt(0) - _gl_xl) * _inv_resolution), 0), _GLX_SIZE - 1); + idx(1) = std::min( std::max( int( (pt(1) - _gl_yl) * _inv_resolution), 0), _GLY_SIZE - 1); + idx(2) = std::min( std::max( int( (pt(2) - _gl_zl) * _inv_resolution), 0), _GLZ_SIZE - 1); + + return idx; +}; + +void rcvOdometryCallbck(const nav_msgs::Odometry& odom) +{ + /*if(!has_global_map) + return;*/ + has_odom = true; + _odom = odom; + Matrix4d Pose_receive = Matrix4d::Identity(); + + Eigen::Vector3d request_position; + Eigen::Quaterniond request_pose; + request_position.x() = odom.pose.pose.position.x; + request_position.y() = odom.pose.pose.position.y; + request_position.z() = odom.pose.pose.position.z; + request_pose.x() = odom.pose.pose.orientation.x; + request_pose.y() = odom.pose.pose.orientation.y; + request_pose.z() = odom.pose.pose.orientation.z; + request_pose.w() = odom.pose.pose.orientation.w; + Pose_receive.block<3,3>(0,0) = request_pose.toRotationMatrix(); + Pose_receive(0,3) = request_position(0); + Pose_receive(1,3) = request_position(1); + Pose_receive(2,3) = request_position(2); + + Matrix4d body_pose = Pose_receive; + //convert to cam pose + cam2world = body_pose * cam02body; + cam2world_quat = cam2world.block<3,3>(0,0); + + last_odom_stamp = odom.header.stamp; + + last_pose_world(0) = odom.pose.pose.position.x; + last_pose_world(1) = odom.pose.pose.position.y; + last_pose_world(2) = odom.pose.pose.position.z; + + //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, last_odom_stamp, "world", "camera")); //publish transform from world frame to quadrotor frame.*/ +} + +void pubCameraPose(const ros::TimerEvent & event) +{ + //cout<<"pub cam pose" + geometry_msgs::PoseStamped camera_pose; + camera_pose.header = _odom.header; + camera_pose.header.frame_id = "/map"; + camera_pose.pose.position.x = cam2world(0,3); + camera_pose.pose.position.y = cam2world(1,3); + camera_pose.pose.position.z = cam2world(2,3); + camera_pose.pose.orientation.w = cam2world_quat.w(); + camera_pose.pose.orientation.x = cam2world_quat.x(); + camera_pose.pose.orientation.y = cam2world_quat.y(); + camera_pose.pose.orientation.z = cam2world_quat.z(); + pub_pose.publish(camera_pose); +} + +void renderSensedPoints(const ros::TimerEvent & event) +{ + //if(! has_global_map || ! has_odom) return; + if( !has_global_map && !has_local_map) return; + + if( !has_odom ) return; + render_currentpose(); + render_pcl_world(); +} + +vector cloud_data; +void rcvGlobalPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map ) +{ + if(has_global_map) + return; + + ROS_WARN("Global Pointcloud received.."); + //load global map + pcl::PointCloud cloudIn; + pcl::PointXYZ pt_in; + //transform map to point cloud format + pcl::fromROSMsg(pointcloud_map, cloudIn); + for(int i = 0; i < int(cloudIn.points.size()); i++){ + pt_in = cloudIn.points[i]; + cloud_data.push_back(pt_in.x); + cloud_data.push_back(pt_in.y); + cloud_data.push_back(pt_in.z); + } + printf("global map has points: %d.\n", (int)cloud_data.size() / 3 ); + //pass cloud_data to depth render + depthrender.set_data(cloud_data); + depth_hostptr = (int*) malloc(width * height * sizeof(int)); + + has_global_map = true; +} + +void rcvLocalPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map ) +{ + //ROS_WARN("Local Pointcloud received.."); + //load local map + pcl::PointCloud cloudIn; + pcl::PointXYZ pt_in; + //transform map to point cloud format + pcl::fromROSMsg(pointcloud_map, cloudIn); + + if(cloudIn.points.size() == 0) return; + for(int i = 0; i < int(cloudIn.points.size()); i++){ + pt_in = cloudIn.points[i]; + Eigen::Vector3d pose_pt(pt_in.x, pt_in.y, pt_in.z); + //pose_pt = gridIndex2coord(coord2gridIndex(pose_pt)); + cloud_data.push_back(pose_pt(0)); + cloud_data.push_back(pose_pt(1)); + cloud_data.push_back(pose_pt(2)); + } + //printf("local map has points: %d.\n", (int)cloud_data.size() / 3 ); + //pass cloud_data to depth render + depthrender.set_data(cloud_data); + depth_hostptr = (int*) malloc(width * height * sizeof(int)); + + has_local_map = true; +} + +void render_pcl_world() +{ + //for debug purpose + pcl::PointCloud localMap; + pcl::PointXYZ pt_in; + + Eigen::Vector4d pose_in_camera; + Eigen::Vector4d pose_in_world; + Eigen::Vector3d pose_pt; + + for(int u = 0; u < width; u++) + for(int v = 0; v < height; v++){ + float depth = depth_mat.at(v,u); + + if(depth == 0.0) + continue; + + 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; + + pose_in_world = cam2world * pose_in_camera; + + if( (pose_in_world.segment(0,3) - last_pose_world).norm() > sensing_horizon ) + continue; + + pose_pt = pose_in_world.head(3); + //pose_pt = gridIndex2coord(coord2gridIndex(pose_pt)); + pt_in.x = pose_pt(0); + pt_in.y = pose_pt(1); + pt_in.z = pose_pt(2); + + localMap.points.push_back(pt_in); + } + + localMap.width = localMap.points.size(); + localMap.height = 1; + localMap.is_dense = true; + + pcl::toROSMsg(localMap, local_map_pcl); + local_map_pcl.header.frame_id = "/map"; + local_map_pcl.header.stamp = last_odom_stamp; + + pub_pcl_wolrd.publish(local_map_pcl); +} + +void render_currentpose() +{ + double this_time = ros::Time::now().toSec(); + + Matrix4d cam_pose = cam2world.inverse(); + + double pose[4 * 4]; + + for(int i = 0; i < 4; i ++) + for(int j = 0; j < 4; j ++) + pose[j + 4 * i] = cam_pose(i, j); + + depthrender.render_pose(pose, depth_hostptr); + //depthrender.render_pose(cam_pose, depth_hostptr); + + depth_mat = cv::Mat::zeros(height, width, CV_32FC1); + double min = 0.5; + double max = 1.0f; + for(int i = 0; i < height; i++) + for(int j = 0; j < width; j++) + { + float depth = (float)depth_hostptr[i * width + j] / 1000.0f; + depth = depth < 500.0f ? depth : 0; + max = depth > max ? depth : max; + depth_mat.at(i,j) = depth; + } + //ROS_INFO("render cost %lf ms.", (ros::Time::now().toSec() - this_time) * 1000.0f); + //printf("max_depth %lf.\n", max); + + cv_bridge::CvImage out_msg; + out_msg.header.stamp = last_odom_stamp; + out_msg.header.frame_id = "camera"; + out_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + out_msg.image = depth_mat.clone(); + pub_depth.publish(out_msg.toImageMsg()); + + cv::Mat adjMap; + // depth_mat.convertTo(adjMap,CV_8UC1, 255 / (max-min), -min); + depth_mat.convertTo(adjMap,CV_8UC1, 255 /13.0, -min); + cv::Mat falseColorsMap; + cv::applyColorMap(adjMap, falseColorsMap, cv::COLORMAP_RAINBOW); + cv_bridge::CvImage cv_image_colored; + cv_image_colored.header.frame_id = "depthmap"; + cv_image_colored.header.stamp = last_odom_stamp; + cv_image_colored.encoding = sensor_msgs::image_encodings::BGR8; + cv_image_colored.image = falseColorsMap; + pub_color.publish(cv_image_colored.toImageMsg()); + //cv::imshow("depth_image", adjMap); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "pcl_render"); + ros::NodeHandle nh("~"); + + nh.getParam("cam_width", width); + nh.getParam("cam_height", height); + nh.getParam("cam_fx", fx); + nh.getParam("cam_fy", fy); + nh.getParam("cam_cx", cx); + nh.getParam("cam_cy", cy); + nh.getParam("sensing_horizon", sensing_horizon); + nh.getParam("sensing_rate", sensing_rate); + nh.getParam("estimation_rate", estimation_rate); + + nh.getParam("map/x_size", _x_size); + nh.getParam("map/y_size", _y_size); + nh.getParam("map/z_size", _z_size); + + depthrender.set_para(fx, fy, cx, cy, width, height); + + // cam02body << 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, + // 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, + // -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, + // 0.0, 0.0, 0.0, 1.0; + + cam02body << 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 cam2world transformation + cam2world = Matrix4d::Identity(); + //subscribe point cloud + global_map_sub = nh.subscribe( "global_map", 1, rcvGlobalPointCloudCallBack); + local_map_sub = nh.subscribe( "local_map", 1, rcvLocalPointCloudCallBack); + odom_sub = nh.subscribe( "odometry", 50, rcvOdometryCallbck ); + + //publisher depth image and color image + pub_depth = nh.advertise("depth",1000); + pub_color = nh.advertise("colordepth",1000); + pub_pose = nh.advertise("camera_pose",1000); + pub_pcl_wolrd = nh.advertise("rendered_pcl",1); + + double sensing_duration = 1.0 / sensing_rate; + double estimate_duration = 1.0 / estimation_rate; + + local_sensing_timer = nh.createTimer(ros::Duration(sensing_duration), renderSensedPoints); + estimation_timer = nh.createTimer(ros::Duration(estimate_duration), pubCameraPose); + //cv::namedWindow("depth_image",1); + + _inv_resolution = 1.0 / _resolution; + + _gl_xl = -_x_size/2.0; + _gl_yl = -_y_size/2.0; + _gl_zl = 0.0; + + _GLX_SIZE = (int)(_x_size * _inv_resolution); + _GLY_SIZE = (int)(_y_size * _inv_resolution); + _GLZ_SIZE = (int)(_z_size * _inv_resolution); + + ros::Rate rate(100); + bool status = ros::ok(); + while(status) + { + ros::spinOnce(); + status = ros::ok(); + rate.sleep(); + } +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/pointcloud_render_node.cpp b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/pointcloud_render_node.cpp new file mode 100644 index 0000000..7668d29 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/local_sensing/src/pointcloud_render_node.cpp @@ -0,0 +1,202 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace Eigen; + +ros::Publisher pub_cloud; + +sensor_msgs::PointCloud2 local_map_pcl; +sensor_msgs::PointCloud2 local_depth_pcl; + +ros::Subscriber odom_sub; +ros::Subscriber global_map_sub, local_map_sub; + +ros::Timer local_sensing_timer; + +bool has_global_map(false); +bool has_local_map(false); +bool has_odom(false); + +nav_msgs::Odometry _odom; + +double sensing_horizon, sensing_rate, estimation_rate; +double _x_size, _y_size, _z_size; +double _gl_xl, _gl_yl, _gl_zl; +double _resolution, _inv_resolution; +int _GLX_SIZE, _GLY_SIZE, _GLZ_SIZE; + +ros::Time last_odom_stamp = ros::TIME_MAX; + +inline Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i& index) { + Eigen::Vector3d pt; + pt(0) = ((double)index(0) + 0.5) * _resolution + _gl_xl; + pt(1) = ((double)index(1) + 0.5) * _resolution + _gl_yl; + pt(2) = ((double)index(2) + 0.5) * _resolution + _gl_zl; + + return pt; +}; + +inline Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d& pt) { + Eigen::Vector3i idx; + idx(0) = std::min(std::max(int((pt(0) - _gl_xl) * _inv_resolution), 0), + _GLX_SIZE - 1); + idx(1) = std::min(std::max(int((pt(1) - _gl_yl) * _inv_resolution), 0), + _GLY_SIZE - 1); + idx(2) = std::min(std::max(int((pt(2) - _gl_zl) * _inv_resolution), 0), + _GLZ_SIZE - 1); + + return idx; +}; + +void rcvOdometryCallbck(const nav_msgs::Odometry& odom) { + /*if(!has_global_map) + return;*/ + has_odom = true; + _odom = odom; +} + +pcl::PointCloud _cloud_all_map, _local_map; +pcl::VoxelGrid _voxel_sampler; +sensor_msgs::PointCloud2 _local_map_pcd; + +pcl::search::KdTree _kdtreeLocalMap; +vector _pointIdxRadiusSearch; +vector _pointRadiusSquaredDistance; + +void rcvGlobalPointCloudCallBack( + const sensor_msgs::PointCloud2& pointcloud_map) { + if (has_global_map) return; + + ROS_WARN("Global Pointcloud received.."); + + pcl::PointCloud cloud_input; + pcl::fromROSMsg(pointcloud_map, cloud_input); + + _voxel_sampler.setLeafSize(0.1f, 0.1f, 0.1f); + _voxel_sampler.setInputCloud(cloud_input.makeShared()); + _voxel_sampler.filter(_cloud_all_map); + + _kdtreeLocalMap.setInputCloud(_cloud_all_map.makeShared()); + + has_global_map = true; +} + +void renderSensedPoints(const ros::TimerEvent& event) { + if (!has_global_map || !has_odom) return; + + Eigen::Quaterniond q; + q.x() = _odom.pose.pose.orientation.x; + q.y() = _odom.pose.pose.orientation.y; + q.z() = _odom.pose.pose.orientation.z; + q.w() = _odom.pose.pose.orientation.w; + + Eigen::Matrix3d rot; + rot = q; + Eigen::Vector3d yaw_vec = rot.col(0); + + _local_map.points.clear(); + pcl::PointXYZ searchPoint(_odom.pose.pose.position.x, + _odom.pose.pose.position.y, + _odom.pose.pose.position.z); + _pointIdxRadiusSearch.clear(); + _pointRadiusSquaredDistance.clear(); + + pcl::PointXYZ pt; + if (_kdtreeLocalMap.radiusSearch(searchPoint, sensing_horizon, + _pointIdxRadiusSearch, + _pointRadiusSquaredDistance) > 0) { + for (size_t i = 0; i < _pointIdxRadiusSearch.size(); ++i) { + pt = _cloud_all_map.points[_pointIdxRadiusSearch[i]]; + + // if ((fabs(pt.z - _odom.pose.pose.position.z) / (pt.x - _odom.pose.pose.position.x)) > + // tan(M_PI / 12.0)) + // continue; + if ((fabs(pt.z - _odom.pose.pose.position.z) / sensing_horizon) > + tan(M_PI / 6.0)) + continue; + + Vector3d pt_vec(pt.x - _odom.pose.pose.position.x, + pt.y - _odom.pose.pose.position.y, + pt.z - _odom.pose.pose.position.z); + + if (pt_vec.normalized().dot(yaw_vec) < 0.5) continue; + + _local_map.points.push_back(pt); + } + } else { + return; + } + + _local_map.width = _local_map.points.size(); + _local_map.height = 1; + _local_map.is_dense = true; + + pcl::toROSMsg(_local_map, _local_map_pcd); + _local_map_pcd.header.frame_id = "map"; + + pub_cloud.publish(_local_map_pcd); +} + +void rcvLocalPointCloudCallBack( + const sensor_msgs::PointCloud2& pointcloud_map) { + // do nothing, fix later +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "pcl_render"); + ros::NodeHandle nh("~"); + + nh.getParam("sensing_horizon", sensing_horizon); + nh.getParam("sensing_rate", sensing_rate); + nh.getParam("estimation_rate", estimation_rate); + + nh.getParam("map/x_size", _x_size); + nh.getParam("map/y_size", _y_size); + nh.getParam("map/z_size", _z_size); + + // subscribe point cloud + global_map_sub = nh.subscribe("global_map", 1, rcvGlobalPointCloudCallBack); + local_map_sub = nh.subscribe("local_map", 1, rcvLocalPointCloudCallBack); + odom_sub = nh.subscribe("odometry", 50, rcvOdometryCallbck); + + // publisher depth image and color image + pub_cloud = + nh.advertise("pcl_render_node/cloud", 10); + + double sensing_duration = 1.0 / sensing_rate * 2.5; + + local_sensing_timer = + nh.createTimer(ros::Duration(sensing_duration), renderSensedPoints); + + _inv_resolution = 1.0 / _resolution; + + _gl_xl = -_x_size / 2.0; + _gl_yl = -_y_size / 2.0; + _gl_zl = 0.0; + + _GLX_SIZE = (int)(_x_size * _inv_resolution); + _GLY_SIZE = (int)(_y_size * _inv_resolution); + _GLZ_SIZE = (int)(_z_size * _inv_resolution); + + ros::Rate rate(100); + bool status = ros::ok(); + while (status) { + ros::spinOnce(); + status = ros::ok(); + rate.sleep(); + } +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/map_generator/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/map_generator/CMakeLists.txt new file mode 100644 index 0000000..4567da1 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/map_generator/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 2.8.3) +project(map_generator) + +set(CMAKE_BUILD_TYPE "Release") +#set(CMAKE_CXX_FLAGS "-std=c++11") +ADD_COMPILE_OPTIONS(-std=c++11 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + geometry_msgs + pcl_conversions +) +find_package(PCL REQUIRED) +find_package(Eigen3 REQUIRED) + +catkin_package() + +include_directories( +# include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} +) + +add_executable (random_forest src/random_forest_sensing.cpp ) +target_link_libraries(random_forest + ${catkin_LIBRARIES} + ${PCL_LIBRARIES}) diff --git a/motion_planning/3d/ego_planner/uav_simulator/map_generator/package.xml b/motion_planning/3d/ego_planner/uav_simulator/map_generator/package.xml new file mode 100644 index 0000000..42f7c33 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/map_generator/package.xml @@ -0,0 +1,65 @@ + + + map_generator + 0.0.0 + The map_generator package + + + + + bzhouai + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + roscpp + std_msgs + roscpp + std_msgs + + + + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/map_generator/src/random_forest_sensing.cpp b/motion_planning/3d/ego_planner/uav_simulator/map_generator/src/random_forest_sensing.cpp new file mode 100644 index 0000000..e5dd4be --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/map_generator/src/random_forest_sensing.cpp @@ -0,0 +1,436 @@ +#include +#include +// #include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +// pcl::search::KdTree kdtreeLocalMap; +pcl::KdTreeFLANN kdtreeLocalMap; +vector pointIdxRadiusSearch; +vector pointRadiusSquaredDistance; + +random_device rd; +// default_random_engine eng(4); +default_random_engine eng(rd()); +uniform_real_distribution rand_x; +uniform_real_distribution rand_y; +uniform_real_distribution rand_w; +uniform_real_distribution rand_h; +uniform_real_distribution rand_inf; + +ros::Publisher _local_map_pub; +ros::Publisher _all_map_pub; +ros::Publisher click_map_pub_; +ros::Subscriber _odom_sub; + +vector _state; + +int _obs_num; +double _x_size, _y_size, _z_size; +double _x_l, _x_h, _y_l, _y_h, _w_l, _w_h, _h_l, _h_h; +double _z_limit, _sensing_range, _resolution, _sense_rate, _init_x, _init_y; +double _min_dist; + +bool _map_ok = false; +bool _has_odom = false; + +int circle_num_; +double radius_l_, radius_h_, z_l_, z_h_; +double theta_; +uniform_real_distribution rand_radius_; +uniform_real_distribution rand_radius2_; +uniform_real_distribution rand_theta_; +uniform_real_distribution rand_z_; + +sensor_msgs::PointCloud2 globalMap_pcd; +pcl::PointCloud cloudMap; + +sensor_msgs::PointCloud2 localMap_pcd; +pcl::PointCloud clicked_cloud_; + +void RandomMapGenerate() { + pcl::PointXYZ pt_random; + + rand_x = uniform_real_distribution(_x_l, _x_h); + rand_y = uniform_real_distribution(_y_l, _y_h); + rand_w = uniform_real_distribution(_w_l, _w_h); + rand_h = uniform_real_distribution(_h_l, _h_h); + + rand_radius_ = uniform_real_distribution(radius_l_, radius_h_); + rand_radius2_ = uniform_real_distribution(radius_l_, 1.2); + rand_theta_ = uniform_real_distribution(-theta_, theta_); + rand_z_ = uniform_real_distribution(z_l_, z_h_); + + // generate polar obs + for (int i = 0; i < _obs_num; i++) { + double x, y, w, h; + x = rand_x(eng); + y = rand_y(eng); + w = rand_w(eng); + + x = floor(x / _resolution) * _resolution + _resolution / 2.0; + y = floor(y / _resolution) * _resolution + _resolution / 2.0; + + int widNum = ceil(w / _resolution); + + for (int r = -widNum / 2.0; r < widNum / 2.0; r++) + for (int s = -widNum / 2.0; s < widNum / 2.0; s++) { + h = rand_h(eng); + int heiNum = ceil(h / _resolution); + for (int t = -20; t < heiNum; t++) { + pt_random.x = x + (r + 0.5) * _resolution + 1e-2; + pt_random.y = y + (s + 0.5) * _resolution + 1e-2; + pt_random.z = (t + 0.5) * _resolution + 1e-2; + cloudMap.points.push_back(pt_random); + } + } + } + + // generate circle obs + for (int i = 0; i < circle_num_; ++i) { + double x, y, z; + x = rand_x(eng); + y = rand_y(eng); + z = rand_z_(eng); + + x = floor(x / _resolution) * _resolution + _resolution / 2.0; + y = floor(y / _resolution) * _resolution + _resolution / 2.0; + z = floor(z / _resolution) * _resolution + _resolution / 2.0; + + Eigen::Vector3d translate(x, y, z); + + double theta = rand_theta_(eng); + Eigen::Matrix3d rotate; + rotate << cos(theta), -sin(theta), 0.0, sin(theta), cos(theta), 0.0, 0, 0, + 1; + + double radius1 = rand_radius_(eng); + double radius2 = rand_radius2_(eng); + + // draw a circle centered at (x,y,z) + Eigen::Vector3d cpt; + for (double angle = 0.0; angle < 6.282; angle += _resolution / 2) { + cpt(0) = 0.0; + cpt(1) = radius1 * cos(angle); + cpt(2) = radius2 * sin(angle); + + // inflate + Eigen::Vector3d cpt_if; + for (int ifx = -0; ifx <= 0; ++ifx) + for (int ify = -0; ify <= 0; ++ify) + for (int ifz = -0; ifz <= 0; ++ifz) { + cpt_if = cpt + Eigen::Vector3d(ifx * _resolution, ify * _resolution, + ifz * _resolution); + cpt_if = rotate * cpt_if + Eigen::Vector3d(x, y, z); + pt_random.x = cpt_if(0); + pt_random.y = cpt_if(1); + pt_random.z = cpt_if(2); + cloudMap.push_back(pt_random); + } + } + } + + cloudMap.width = cloudMap.points.size(); + cloudMap.height = 1; + cloudMap.is_dense = true; + + ROS_WARN("Finished generate random map "); + + kdtreeLocalMap.setInputCloud(cloudMap.makeShared()); + + _map_ok = true; +} + +void RandomMapGenerateCylinder() { + pcl::PointXYZ pt_random; + + vector obs_position; + + rand_x = uniform_real_distribution(_x_l, _x_h); + rand_y = uniform_real_distribution(_y_l, _y_h); + rand_w = uniform_real_distribution(_w_l, _w_h); + rand_h = uniform_real_distribution(_h_l, _h_h); + rand_inf = uniform_real_distribution(0.5, 1.5); + + rand_radius_ = uniform_real_distribution(radius_l_, radius_h_); + rand_radius2_ = uniform_real_distribution(radius_l_, 1.2); + rand_theta_ = uniform_real_distribution(-theta_, theta_); + rand_z_ = uniform_real_distribution(z_l_, z_h_); + + // generate polar obs + for (int i = 0; i < _obs_num && ros::ok(); i++) { + double x, y, w, h, inf; + x = rand_x(eng); + y = rand_y(eng); + w = rand_w(eng); + inf = rand_inf(eng); + + bool flag_continue = false; + for ( auto p : obs_position ) + if ( (Eigen::Vector2d(x,y) - p).norm() < _min_dist /*metres*/ ) + { + i--; + flag_continue = true; + break; + } + if ( flag_continue ) continue; + + obs_position.push_back( Eigen::Vector2d(x,y) ); + + + x = floor(x / _resolution) * _resolution + _resolution / 2.0; + y = floor(y / _resolution) * _resolution + _resolution / 2.0; + + int widNum = ceil((w*inf) / _resolution); + double radius = (w*inf) / 2; + + for (int r = -widNum / 2.0; r < widNum / 2.0; r++) + for (int s = -widNum / 2.0; s < widNum / 2.0; s++) { + h = rand_h(eng); + int heiNum = ceil(h / _resolution); + for (int t = -20; t < heiNum; t++) { + double temp_x = x + (r + 0.5) * _resolution + 1e-2; + double temp_y = y + (s + 0.5) * _resolution + 1e-2; + double temp_z = (t + 0.5) * _resolution + 1e-2; + if ( (Eigen::Vector2d(temp_x,temp_y) - Eigen::Vector2d(x,y)).norm() <= radius ) + { + pt_random.x = temp_x; + pt_random.y = temp_y; + pt_random.z = temp_z; + cloudMap.points.push_back(pt_random); + } + } + } + } + + // generate circle obs + for (int i = 0; i < circle_num_; ++i) { + double x, y, z; + x = rand_x(eng); + y = rand_y(eng); + z = rand_z_(eng); + + x = floor(x / _resolution) * _resolution + _resolution / 2.0; + y = floor(y / _resolution) * _resolution + _resolution / 2.0; + z = floor(z / _resolution) * _resolution + _resolution / 2.0; + + Eigen::Vector3d translate(x, y, z); + + double theta = rand_theta_(eng); + Eigen::Matrix3d rotate; + rotate << cos(theta), -sin(theta), 0.0, sin(theta), cos(theta), 0.0, 0, 0, + 1; + + double radius1 = rand_radius_(eng); + double radius2 = rand_radius2_(eng); + + // draw a circle centered at (x,y,z) + Eigen::Vector3d cpt; + for (double angle = 0.0; angle < 6.282; angle += _resolution / 2) { + cpt(0) = 0.0; + cpt(1) = radius1 * cos(angle); + cpt(2) = radius2 * sin(angle); + + // inflate + Eigen::Vector3d cpt_if; + for (int ifx = -0; ifx <= 0; ++ifx) + for (int ify = -0; ify <= 0; ++ify) + for (int ifz = -0; ifz <= 0; ++ifz) { + cpt_if = cpt + Eigen::Vector3d(ifx * _resolution, ify * _resolution, + ifz * _resolution); + cpt_if = rotate * cpt_if + Eigen::Vector3d(x, y, z); + pt_random.x = cpt_if(0); + pt_random.y = cpt_if(1); + pt_random.z = cpt_if(2); + cloudMap.push_back(pt_random); + } + } + } + + cloudMap.width = cloudMap.points.size(); + cloudMap.height = 1; + cloudMap.is_dense = true; + + ROS_WARN("Finished generate random map "); + + kdtreeLocalMap.setInputCloud(cloudMap.makeShared()); + + _map_ok = true; +} + +void rcvOdometryCallbck(const nav_msgs::Odometry odom) { + if (odom.child_frame_id == "X" || odom.child_frame_id == "O") return; + _has_odom = true; + + _state = {odom.pose.pose.position.x, + odom.pose.pose.position.y, + odom.pose.pose.position.z, + odom.twist.twist.linear.x, + odom.twist.twist.linear.y, + odom.twist.twist.linear.z, + 0.0, + 0.0, + 0.0}; +} + +int i = 0; +void pubSensedPoints() { + // if (i < 10) { + pcl::toROSMsg(cloudMap, globalMap_pcd); + globalMap_pcd.header.frame_id = "world"; + _all_map_pub.publish(globalMap_pcd); + // } + + return; + + /* ---------- only publish points around current position ---------- */ + if (!_map_ok || !_has_odom) return; + + pcl::PointCloud localMap; + + pcl::PointXYZ searchPoint(_state[0], _state[1], _state[2]); + pointIdxRadiusSearch.clear(); + pointRadiusSquaredDistance.clear(); + + pcl::PointXYZ pt; + + if (isnan(searchPoint.x) || isnan(searchPoint.y) || isnan(searchPoint.z)) + return; + + if (kdtreeLocalMap.radiusSearch(searchPoint, _sensing_range, + pointIdxRadiusSearch, + pointRadiusSquaredDistance) > 0) { + for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) { + pt = cloudMap.points[pointIdxRadiusSearch[i]]; + localMap.points.push_back(pt); + } + } else { + ROS_ERROR("[Map server] No obstacles ."); + return; + } + + localMap.width = localMap.points.size(); + localMap.height = 1; + localMap.is_dense = true; + + pcl::toROSMsg(localMap, localMap_pcd); + localMap_pcd.header.frame_id = "world"; + _local_map_pub.publish(localMap_pcd); +} + +void clickCallback(const geometry_msgs::PoseStamped& msg) { + double x = msg.pose.position.x; + double y = msg.pose.position.y; + double w = rand_w(eng); + double h; + pcl::PointXYZ pt_random; + + x = floor(x / _resolution) * _resolution + _resolution / 2.0; + y = floor(y / _resolution) * _resolution + _resolution / 2.0; + + int widNum = ceil(w / _resolution); + + for (int r = -widNum / 2.0; r < widNum / 2.0; r++) + for (int s = -widNum / 2.0; s < widNum / 2.0; s++) { + h = rand_h(eng); + int heiNum = ceil(h / _resolution); + for (int t = -1; t < heiNum; t++) { + pt_random.x = x + (r + 0.5) * _resolution + 1e-2; + pt_random.y = y + (s + 0.5) * _resolution + 1e-2; + pt_random.z = (t + 0.5) * _resolution + 1e-2; + clicked_cloud_.points.push_back(pt_random); + cloudMap.points.push_back(pt_random); + } + } + clicked_cloud_.width = clicked_cloud_.points.size(); + clicked_cloud_.height = 1; + clicked_cloud_.is_dense = true; + + pcl::toROSMsg(clicked_cloud_, localMap_pcd); + localMap_pcd.header.frame_id = "world"; + click_map_pub_.publish(localMap_pcd); + + cloudMap.width = cloudMap.points.size(); + + return; +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "random_map_sensing"); + ros::NodeHandle n("~"); + + _local_map_pub = n.advertise("/map_generator/local_cloud", 1); + _all_map_pub = n.advertise("/map_generator/global_cloud", 1); + + _odom_sub = n.subscribe("odometry", 50, rcvOdometryCallbck); + + click_map_pub_ = + n.advertise("/pcl_render_node/local_map", 1); + // ros::Subscriber click_sub = n.subscribe("/goal", 10, clickCallback); + + n.param("init_state_x", _init_x, 0.0); + n.param("init_state_y", _init_y, 0.0); + + n.param("map/x_size", _x_size, 50.0); + n.param("map/y_size", _y_size, 50.0); + n.param("map/z_size", _z_size, 5.0); + n.param("map/obs_num", _obs_num, 30); + n.param("map/resolution", _resolution, 0.1); + n.param("map/circle_num", circle_num_, 30); + + n.param("ObstacleShape/lower_rad", _w_l, 0.3); + n.param("ObstacleShape/upper_rad", _w_h, 0.8); + n.param("ObstacleShape/lower_hei", _h_l, 3.0); + n.param("ObstacleShape/upper_hei", _h_h, 7.0); + + n.param("ObstacleShape/radius_l", radius_l_, 7.0); + n.param("ObstacleShape/radius_h", radius_h_, 7.0); + n.param("ObstacleShape/z_l", z_l_, 7.0); + n.param("ObstacleShape/z_h", z_h_, 7.0); + n.param("ObstacleShape/theta", theta_, 7.0); + + n.param("sensing/radius", _sensing_range, 10.0); + n.param("sensing/rate", _sense_rate, 10.0); + + n.param("min_distance", _min_dist, 1.0); + + _x_l = -_x_size / 2.0; + _x_h = +_x_size / 2.0; + + _y_l = -_y_size / 2.0; + _y_h = +_y_size / 2.0; + + _obs_num = min(_obs_num, (int)_x_size * 10); + _z_limit = _z_size; + + ros::Duration(0.5).sleep(); + + // unsigned int seed = rd(); + unsigned int seed = 2433201515; + cout << "seed=" << seed << endl; + eng.seed(seed); + + // RandomMapGenerate(); + RandomMapGenerateCylinder(); + + ros::Rate loop_rate(_sense_rate); + + while (ros::ok()) { + pubSensedPoints(); + ros::spinOnce(); + loop_rate.sleep(); + } +} \ No newline at end of file diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/mockamap/CMakeLists.txt new file mode 100644 index 0000000..2593c16 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/CMakeLists.txt @@ -0,0 +1,207 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mockamap) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +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 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 + pcl_ros + pcl_conversions +) + +## 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 tag for "message_generation" +## * 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 dependency has been pulled in +## but can be declared for certainty nonetheless: +## * 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 +# Message1.msg +# Message2.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 +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## 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 mockamap + CATKIN_DEPENDS roscpp pcl_ros pcl_conversions +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/mockamap.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +file(GLOB ${PROJECT_NAME}_SRCS src/*.cpp) + +add_executable(${PROJECT_NAME}_node + ${${PROJECT_NAME}_SRCS}) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against + target_link_libraries(${PROJECT_NAME}_node + ${catkin_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 ${PROJECT_NAME} ${PROJECT_NAME}_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_mockamap.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) diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/config/rviz.rviz b/motion_planning/3d/ego_planner/uav_simulator/mockamap/config/rviz.rviz new file mode 100644 index 0000000..f69d35e --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/config/rviz.rviz @@ -0,0 +1,217 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Path1 + - /PointCloud22 + Splitter Ratio: 0.5 + Tree Height: 489 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + - /Game Like Input1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: mockamap/PCDView + Input: /mock_map + Name: PCDView + Output: /mock_map/cut +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.9 + Min Value: 0 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Boxes + Topic: /mock_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /theta_star_node/output + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.9 + Min Value: 0 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Boxes + Topic: /mock_map/cut + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + - Boost Property: 0.5 + Class: rviz/FPSMotionTool + Fallback Tool: rviz/Interact + Fallback ViewController: rviz/Orbit + Fly Mode: false + Left Hand Mode: false + Step Length: 0.1 + - Class: rviz_plugins/GameLikeInput + RangeX_max: 1000 + RangeX_min: -1000 + RangeY_max: 1000 + RangeY_min: -1000 + RangeZ_max: 1000 + RangeZ_min: -1000 + TopicPoint: point_list + TopicSelect: select_list + TopicSwarm: swarm + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 9.57022 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -2.02708 + Y: -4.23441 + Z: 0.814007 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.525203 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.88225 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + PCDView: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000027000000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000002800000296000000fb0100001dfa000000010100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000009800fffffffb000000100044006900730070006c00610079007301000000000000016a0000016a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f0070000000024d0000009e0000000000000000fb0000000e005000430044005600690065007701000002c4000000fa000000cd00ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003b40000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1855 + X: 65 + Y: 24 diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/include/maps.hpp b/motion_planning/3d/ego_planner/uav_simulator/mockamap/include/maps.hpp new file mode 100644 index 0000000..bcda759 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/include/maps.hpp @@ -0,0 +1,73 @@ +#ifndef MAPS_HPP +#define MAPS_HPP + +#include + +#include +#include +#include + +namespace mocka { + +class Maps { +public: + typedef struct BasicInfo { + ros::NodeHandle *nh_private; + int sizeX; + int sizeY; + int sizeZ; + int seed; + double scale; + sensor_msgs::PointCloud2 *output; + pcl::PointCloud *cloud; + } BasicInfo; + + BasicInfo getInfo() const; + void setInfo(const BasicInfo &value); + +public: + Maps(); + +public: + void generate(int type); + +private: + BasicInfo info; + +private: + void pcl2ros(); + + void perlin3D(); + void maze2D(); + void randomMapGenerate(); + void Maze3DGen(); + void recursiveDivision(int xl, int xh, int yl, int yh, Eigen::MatrixXi &maze); + void recursizeDivisionMaze(Eigen::MatrixXi &maze); + void optimizeMap(); +}; + +class MazePoint { +private: + pcl::PointXYZ point; + double dist1; + double dist2; + int point1; + int point2; + bool isdoor; + +public: + pcl::PointXYZ getPoint(); + int getPoint1(); + int getPoint2(); + double getDist1(); + double getDist2(); + void setPoint(pcl::PointXYZ p); + void setPoint1(int p); + void setPoint2(int p); + void setDist1(double set); + void setDist2(double set); +}; + +} // namespace mocka + +#endif // MAPS_HPP diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/include/perlinnoise.hpp b/motion_planning/3d/ego_planner/uav_simulator/mockamap/include/perlinnoise.hpp new file mode 100644 index 0000000..fd525c8 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/include/perlinnoise.hpp @@ -0,0 +1,33 @@ +#ifndef PERLINNOISE_HPP +#define PERLINNOISE_HPP + +#include + +// THIS CLASS IS A TRANSLATION TO C++11 FROM THE REFERENCE +// JAVA IMPLEMENTATION OF THE IMPROVED PERLIN FUNCTION (see +// http://mrl.nyu.edu/~perlin/noise/) +// THE ORIGINAL JAVA IMPLEMENTATION IS COPYRIGHT 2002 KEN PERLIN + +// I ADDED AN EXTRA METHOD THAT GENERATES A NEW PERMUTATION VECTOR (THIS IS NOT +// PRESENT IN THE ORIGINAL IMPLEMENTATION) + +class PerlinNoise +{ + // The permutation vector + std::vector p; + +public: + // Initialize with the reference values for the permutation vector + PerlinNoise(); + // Generate a new permutation vector based on the value of seed + PerlinNoise(unsigned int seed); + // Get a noise value, for 2D images z can have any value + double noise(double x, double y, double z); + +private: + double fade(double t); + double lerp(double t, double a, double b); + double grad(int hash, double x, double y, double z); +}; + +#endif // PERLINNOISE_HPP diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/maze2d.launch b/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/maze2d.launch new file mode 100644 index 0000000..6eda250 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/maze2d.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/maze3d.launch b/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/maze3d.launch new file mode 100644 index 0000000..aa3c8c1 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/maze3d.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/mockamap.launch b/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/mockamap.launch new file mode 100644 index 0000000..42f9f4e --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/mockamap.launch @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/perlin3d.launch b/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/perlin3d.launch new file mode 100644 index 0000000..2b067f8 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/perlin3d.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/post2d.launch b/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/post2d.launch new file mode 100644 index 0000000..7d20f3e --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/launch/post2d.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/package.xml b/motion_planning/3d/ego_planner/uav_simulator/mockamap/package.xml new file mode 100644 index 0000000..8feb28f --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/package.xml @@ -0,0 +1,57 @@ + + + mockamap + 0.1.0 + The mockamap package + + + + + William.Wu + + + + + + GPLv3 + + + + + + + + + + + + + William.Wu + + + + + + + + + + + + + + catkin + roscpp + pcl_ros + pcl_conversions + roscpp + pcl_ros + pcl_conversions + + + + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/.clang-format b/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/.clang-format new file mode 100644 index 0000000..83360cd --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/.clang-format @@ -0,0 +1,12 @@ +--- +BasedOnStyle: Mozilla +Standard: Cpp03 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: true +AlignConsecutiveDeclarations: true +AllowShortFunctionsOnASingleLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +BreakBeforeBraces: Allman +... + diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/ces_randommap.cpp b/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/ces_randommap.cpp new file mode 100644 index 0000000..ef5d027 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/ces_randommap.cpp @@ -0,0 +1,264 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "visualization_msgs/Marker.h" +#include "visualization_msgs/MarkerArray.h" + +//! @todo historical above +#include "maps.hpp" + +using namespace std; +using namespace mocka; + +#if MAP_OR_WORLD +const string kFrameIdNs_ = "map"; +#else +const string kFrameIdNs_ = "world"; +#endif + +pcl::search::KdTree kdtreeLocalMap; +vector pointIdxRadiusSearch; +vector pointRadiusSquaredDistance; + +ros::Publisher _local_map_pub; +ros::Publisher _local_map_inflate_pub; +ros::Publisher _global_map_pub; + +ros::Subscriber _map_sub; +ros::Subscriber _odom_sub; + +deque _odom_queue; +vector _state; +const size_t _odom_queue_size = 200; +nav_msgs::Odometry _odom; + +double z_limit; +double _SenseRate; +double _sensing_range; + +// ros::Timer vis_map; +bool map_ok = false; +bool _has_odom = false; + +sensor_msgs::PointCloud2 globalMap_pcd; +sensor_msgs::PointCloud2 localMap_pcd; +pcl::PointCloud cloudMap; +ros::Time begin_time = ros::TIME_MAX; + +typedef Eigen::Vector3d ObsPos; +typedef Eigen::Vector3d ObsSize; // x, y, height --- z +typedef pair Obstacle; +std::vector obstacle_list; + +void +fixedMapGenerate() +{ + double _resolution = 1.0; + + cloudMap.points.clear(); + obstacle_list.push_back( + make_pair(ObsPos(-7.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); + obstacle_list.push_back( + make_pair(ObsPos(-1.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); + obstacle_list.push_back( + make_pair(ObsPos(10.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); + obstacle_list.push_back( + make_pair(ObsPos(16.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); + obstacle_list.push_back( + make_pair(ObsPos(-4.0, -1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); + obstacle_list.push_back( + make_pair(ObsPos(13.0, -1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); + + obstacle_list.push_back( + make_pair(ObsPos(5.0, 2.5, 0.0), ObsSize(30.0, 1.0, 5.0))); + obstacle_list.push_back( + make_pair(ObsPos(5.0, -2.5, 0.0), ObsSize(30.0, 1.0, 5.0))); + + int num_total_obs = obstacle_list.size(); + pcl::PointXYZ pt_insert; + + for (int i = 0; i < num_total_obs; i++) + { + double x, y, z; + double lx, ly, lz; + x = (obstacle_list[i].first)[0]; + y = (obstacle_list[i].first)[1]; + z = (obstacle_list[i].first)[2]; + lx = (obstacle_list[i].second)[0]; + ly = (obstacle_list[i].second)[1]; + lz = (obstacle_list[i].second)[2]; + + int num_mesh_x = ceil(lx / _resolution); + int num_mesh_y = ceil(ly / _resolution); + int num_mesh_z = ceil(lz / _resolution); + + int left_x, right_x, left_y, right_y, left_z, right_z; + left_x = -num_mesh_x / 2; + right_x = num_mesh_x / 2; + left_y = -num_mesh_y / 2; + right_y = num_mesh_y / 2; + left_z = 0; + right_z = num_mesh_z; + + for (int r = left_x; r < right_x; r++) + for (int s = left_y; s < right_y; s++) + { + for (int t = left_z; t < right_z; t++) + { + if ((r - left_x) * (r - right_x + 1) * (s - left_y) * + (s - right_y + 1) * (t - left_z) * (t - right_z + 1) == + 0) + { + pt_insert.x = x + r * _resolution; + pt_insert.y = y + s * _resolution; + pt_insert.z = z + t * _resolution; + cloudMap.points.push_back(pt_insert); + } + } + } + } + + cloudMap.width = cloudMap.points.size(); + cloudMap.height = 1; + cloudMap.is_dense = true; + + ROS_WARN("Finished generate random map "); + cout << cloudMap.size() << endl; + kdtreeLocalMap.setInputCloud(cloudMap.makeShared()); + map_ok = true; +} + +void +rcvOdometryCallbck(const nav_msgs::Odometry odom) +{ + if (odom.child_frame_id == "X" || odom.child_frame_id == "O") + return; + _odom = odom; + _has_odom = true; + + _state = { _odom.pose.pose.position.x, + _odom.pose.pose.position.y, + _odom.pose.pose.position.z, + _odom.twist.twist.linear.x, + _odom.twist.twist.linear.y, + _odom.twist.twist.linear.z, + 0.0, + 0.0, + 0.0 }; + + _odom_queue.push_back(odom); + while (_odom_queue.size() > _odom_queue_size) + _odom_queue.pop_front(); +} + +int frequence_division_global = 40; + +void +publishAllPoints() +{ + if (!map_ok) + return; + + if ((ros::Time::now() - begin_time).toSec() > 7.0) + return; + + frequence_division_global--; + if (frequence_division_global == 0) + { + pcl::toROSMsg(cloudMap, globalMap_pcd); + globalMap_pcd.header.frame_id = kFrameIdNs_; + _global_map_pub.publish(globalMap_pcd); + frequence_division_global = 40; + ROS_ERROR("[SERVER]Publish one global map"); + } +} + +void +pubSensedPoints() +{ + if (!map_ok || !_has_odom) + return; + + // ros::Time time_bef_sensing = ros::Time::now(); + + pcl::PointCloud localMap; + + pcl::PointXYZ searchPoint(_state[0], _state[1], _state[2]); + pointIdxRadiusSearch.clear(); + pointRadiusSquaredDistance.clear(); + + pcl::PointXYZ ptInNoflation; + + if (kdtreeLocalMap.radiusSearch(searchPoint, _sensing_range, + pointIdxRadiusSearch, + pointRadiusSquaredDistance) > 0) + { + for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) + { + ptInNoflation = cloudMap.points[pointIdxRadiusSearch[i]]; + localMap.points.push_back(ptInNoflation); + } + } + else + { + // ROS_ERROR("[Map server] No obstacles ."); + // cout< 5.0) + return; + + frequence_division_global--; + if (frequence_division_global == 0) + { + pcl::toROSMsg(cloudMap, globalMap_pcd); + globalMap_pcd.header.frame_id = kFrameIdNs_; + _global_map_pub.publish(globalMap_pcd); + frequence_division_global = 40; + ROS_INFO("[SERVER]Publish one global map"); + } +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/maps.cpp b/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/maps.cpp new file mode 100644 index 0000000..f615b90 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/maps.cpp @@ -0,0 +1,894 @@ +#include "maps.hpp" + +#include +#include +#include +#include + +#include + +#include "perlinnoise.hpp" + +using namespace mocka; + +void +Maps::randomMapGenerate() +{ + + std::default_random_engine eng(info.seed); + + double _resolution = 1 / info.scale; + + double _x_l = -info.sizeX / (2 * info.scale); + double _x_h = info.sizeX / (2 * info.scale); + double _y_l = -info.sizeY / (2 * info.scale); + double _y_h = info.sizeY / (2 * info.scale); + double _h_l = 0; + double _h_h = info.sizeZ / info.scale; + + double _w_l, _w_h; + int _ObsNum; + + info.nh_private->param("width_min", _w_l, 0.6); + info.nh_private->param("width_max", _w_h, 1.5); + info.nh_private->param("obstacle_number", _ObsNum, 10); + + std::uniform_real_distribution rand_x; + std::uniform_real_distribution rand_y; + std::uniform_real_distribution rand_w; + std::uniform_real_distribution rand_h; + + pcl::PointXYZ pt_random; + + rand_x = std::uniform_real_distribution(_x_l, _x_h); + rand_y = std::uniform_real_distribution(_y_l, _y_h); + rand_w = std::uniform_real_distribution(_w_l, _w_h); + rand_h = std::uniform_real_distribution(_h_l, _h_h); + + for (int i = 0; i < _ObsNum; i++) + { + double x, y; + x = rand_x(eng); + y = rand_y(eng); + + double w, h; + w = rand_w(eng); + h = rand_h(eng); + + int widNum = ceil(w / _resolution); + int heiNum = ceil(h / _resolution); + + int rl, rh, sl, sh; + rl = -widNum / 2; + rh = widNum / 2; + sl = -widNum / 2; + sh = widNum / 2; + + for (int r = rl; r < rh; r++) + for (int s = sl; s < sh; s++) + { + for (int t = 0; t < heiNum; t++) + { + if ((r - rl) * (r - rh + 1) * (s - sl) * (s - sh + 1) * t * + (t - heiNum + 1) == + 0) + { + pt_random.x = x + r * _resolution; + pt_random.y = y + s * _resolution; + pt_random.z = t * _resolution; + info.cloud->points.push_back(pt_random); + } + } + } + } + + info.cloud->width = info.cloud->points.size(); + info.cloud->height = 1; + info.cloud->is_dense = true; + + pcl2ros(); +} + +void +Maps::pcl2ros() +{ + pcl::toROSMsg(*info.cloud, *info.output); + info.output->header.frame_id = "world"; + ROS_INFO("finish: infill %lf%%", + info.cloud->width / (1.0 * info.sizeX * info.sizeY * info.sizeZ)); +} + +void +Maps::perlin3D() +{ + double complexity; + double fill; + int fractal; + double attenuation; + + info.nh_private->param("complexity", complexity, 0.142857); + info.nh_private->param("fill", fill, 0.38); + info.nh_private->param("fractal", fractal, 1); + info.nh_private->param("attenuation", attenuation, 0.5); + + info.cloud->width = info.sizeX * info.sizeY * info.sizeZ; + info.cloud->height = 1; + info.cloud->points.resize(info.cloud->width * info.cloud->height); + + PerlinNoise noise(info.seed); + + std::vector* v = new std::vector; + v->reserve(info.cloud->width); + for (int i = 0; i < info.sizeX; ++i) + { + for (int j = 0; j < info.sizeY; ++j) + { + for (int k = 0; k < info.sizeZ; ++k) + { + double tnoise = 0; + for (int it = 1; it <= fractal; ++it) + { + int dfv = pow(2, it); + double ta = attenuation / it; + tnoise += ta * noise.noise(dfv * i * complexity, + dfv * j * complexity, + dfv * k * complexity); + } + v->push_back(tnoise); + } + } + } + std::sort(v->begin(), v->end()); + int tpos = info.cloud->width * (1 - fill); + double tmp = v->at(tpos); + ROS_INFO("threshold: %lf", tmp); + + int pos = 0; + for (int i = 0; i < info.sizeX; ++i) + { + for (int j = 0; j < info.sizeY; ++j) + { + for (int k = 0; k < info.sizeZ; ++k) + { + double tnoise = 0; + for (int it = 1; it <= fractal; ++it) + { + int dfv = pow(2, it); + double ta = attenuation / it; + tnoise += ta * noise.noise(dfv * i * complexity, + dfv * j * complexity, + dfv * k * complexity); + } + if (tnoise > tmp) + { + info.cloud->points[pos].x = + i / info.scale - info.sizeX / (2 * info.scale); + info.cloud->points[pos].y = + j / info.scale - info.sizeY / (2 * info.scale); + info.cloud->points[pos].z = k / info.scale; + pos++; + } + } + } + } + info.cloud->width = pos; + ROS_INFO("the number of points before optimization is %d", info.cloud->width); + info.cloud->points.resize(info.cloud->width * info.cloud->height); + pcl2ros(); +} + +void +Maps::recursiveDivision(int xl, int xh, int yl, int yh, Eigen::MatrixXi& maze) +{ + ROS_INFO( + "generating maze with width %d , height %d", xh - xl + 1, yh - yl + 1); + + if (xl < xh - 3 && yl < yh - 3) + { // the remaining area is larger than or equal to 5*5, need to add both x + // wall and y wall + bool valid = false; // used to judge whether the wall selection is valid + int xm = 0; + int ym = 0; + ROS_INFO("entered 5*5 mode"); + while (valid == false) + { + xm = (std::rand() % (xh - xl - 1) + xl + + 1); // generating random number between xl+1 and xh-1(pointless to + // add a wall at the sides) + ym = (std::rand() % (yh - yl - 1) + yl + + 1); // generating random number between yl+1 and yh-1(pointless to + // add a wall at the sides) + if (xl - 1 >= 0) + { // there is a point at xl-1,ym + if (maze(xl - 1, ym) == 0) + { // this is an opening,need to change random number + continue; + } + } + + else if (xh + 1 <= maze.cols() - 1) + { // there is a point at xh+1,ym + if (maze(xh + 1, ym) == 0) + { // this is an opening,need to change random number + continue; + } + } + + else if (yl - 1 >= 0) + { // there is a point at xm,yl-1 + if (maze(xm, yl - 1) == 0) + { // this is an opening,need to change random number + continue; + } + } + + else if (yh + 1 <= maze.rows() - 1) + { // there is a point at xm,yh+1 + if (maze(xm, yh + 1) == 0) + { // this is an opening,need to change random number + continue; + } + } + + valid = true; + + } // xm and ym are now the valid coordinate of the center of the wall + for (int i = xl; i <= xh; i++) + { + maze(i, ym) = 1; + } + for (int j = yl; j <= yh; j++) + { + maze(xm, j) = 1; + } // adding walls around the center point + int d1 = std::rand() % (xm - xl) + xl; + int d2 = std::rand() % (xh - xm) + xm + 1; + int d3 = std::rand() % (ym - yl) + yl; + int d4 = + std::rand() % (yh - ym) + ym + 1; // generating four possible door points + + int decision = std::rand() % 4; // random selection of three doors + switch (decision) + { + case 0: + maze(d1, ym) = 0; + maze(d2, ym) = 0; + maze(xm, d3) = 0; + break; + + case 1: + maze(d1, ym) = 0; + maze(d2, ym) = 0; + maze(xm, d4) = 0; + break; + + case 2: + maze(d2, ym) = 0; + maze(xm, d3) = 0; + maze(xm, d4) = 0; + break; + + case 3: + maze(d1, ym) = 0; + maze(xm, d3) = 0; + maze(xm, d4) = 0; + break; + } // the doors are opened for this cell + if (yl - 1 >= 0) + { + if (maze(xm, yl - 1) == 0) + { + maze(xm, yl) = 0; + } + } + + if (yh + 1 <= maze.rows() - 1) + { + if (maze(xm, yh + 1) == 0) + { + maze(xm, yh) = 0; + } + } + + if (xl - 1 >= 0) + { + if (maze(xl - 1, ym) == 0) + { + maze(xl, ym) = 0; + } + } + + if (xh + 1 <= maze.cols() - 1) + { + if (maze(xh + 1, ym) == 0) + { + maze(xh, ym) = 0; + } + } + + std::cout << maze << std::endl; + recursiveDivision(xl, xm - 1, yl, ym - 1, maze); + recursiveDivision(xm + 1, xh, yl, ym - 1, maze); + recursiveDivision(xl, xm - 1, ym + 1, yh, maze); + recursiveDivision(xm + 1, xh, ym + 1, yh, maze); + + ROS_INFO("finished generating maze with width %d , height %d", + xh - xl + 1, + yh - yl + 1); + std::cout << maze << std::endl; + return; + } // when the remaining area is larger than or equal to 5*5 + + else if (xl < xh - 2 && yl < yh - 2) + { + // bool valid = false; // used to judge whether the wall selection is valid + int xm = 0; + int ym = 0; + int doorcount = 0; + xm = (std::rand() % (xh - xl - 1) + xl + + 1); // generating random number between xl+1 and xh-1(pointless to + // add a wall at the sides) + ym = + (std::rand() % (yh - yl - 1) + yl + + 1); // generating random number between yl+1 and yh-1(pointless to + // add a wall at the sides) + // xm and ym are now the valid coordinate of the center of the wall + for (int i = xl; i <= xh; i++) + { + maze(i, ym) = 1; + } + for (int j = yl; j <= yh; j++) + { + maze(xm, j) = 1; + } // adding walls around the center point + if (yl - 1 >= 0) + { + if (maze(xm, yl - 1) == 0) + { + maze(xm, yl) = 0; + doorcount++; + } + } + + if (yh + 1 <= maze.rows() - 1) + { + if (maze(xm, yh + 1) == 0) + { + maze(xm, yh) = 0; + doorcount++; + } + } + + if (xl - 1 >= 0) + { + if (maze(xl - 1, ym) == 0) + { + maze(xl, ym) = 0; + doorcount++; + } + } + + if (xh + 1 <= maze.cols() - 1) + { + if (maze(xh + 1, ym) == 0) + { + maze(xh, ym) = 0; + doorcount++; + } + } + + int d1 = std::rand() % (xm - xl) + xl; + int d2 = std::rand() % (xh - xm) + xm + 1; + int d3 = std::rand() % (ym - yl) + yl; + int d4 = + std::rand() % (yh - ym) + ym + 1; // generating four possible door points + + int decision = std::rand() % 4; // random selection of three doors + switch (decision) + { + case 0: + maze(d1, ym) = 0; + maze(d2, ym) = 0; + maze(xm, d3) = 0; + break; + + case 1: + maze(d1, ym) = 0; + maze(d2, ym) = 0; + maze(xm, d4) = 0; + break; + + case 2: + maze(d2, ym) = 0; + maze(xm, d3) = 0; + maze(xm, d4) = 0; + break; + + case 3: + maze(d1, ym) = 0; + maze(xm, d3) = 0; + maze(xm, d4) = 0; + break; + } // the doors are opened for this cell + std::cout << maze << std::endl; + + ROS_INFO("finished generating maze with width %d , height %d", + xh - xl + 1, + yh - yl + 1); + std::cout << maze << std::endl; + return; + } + + else if (xl < xh - 1 && yl < yh - 2) + { // the case of 3*4+ + ROS_INFO("entered 3*4+ mode"); + int doorcount = 0; + int ym = 0; + for (int i = yl; i <= yh; i++) + { + maze(xl + 1, i) = 1; + } // filling a center wall + if (yl - 1 >= 0) + { + if (maze(xl + 1, yl - 1) == 0) + { + maze(xl + 1, yl) = 0; + doorcount++; + } + } + if (yh + 1 <= maze.rows() - 1) + { + if (maze(xl + 1, yh + 1) == 0) + { + maze(xl + 1, yh) = 0; + doorcount++; + } + } // opening doors if the wall blocks the old doors + if (doorcount == 0) + { + ym = std::rand() % (yh - yl + 1) + yl; + maze(xl + 1, ym) = 0; + } + } // the case of 4+*3 + // + else if (xl < xh - 2 && yl < yh - 1) + { // the case of 4+*3 + ROS_INFO("entered 4+*3 mode"); + int doorcount = 0; + int xm = 0; + for (int i = xl; i <= xh; i++) + { + maze(i, yl + 1) = 1; + } // filling a center wall + if (xl - 1 >= 0) + { + if (maze(xl - 1, yl + 1) == 0) + { + maze(xl, yl + 1) = 0; + doorcount++; + } + } + if (xh + 1 <= maze.cols() - 1) + { + if (maze(xh + 1, yl + 1) == 0) + { + maze(xh, yl + 1) = 0; + doorcount++; + } + } // opening doors if the wall blocks the old doors + if (doorcount == 0) + { + xm = std::rand() % (xh - xl + 1) + xl; + maze(xm, yl + 1) = 0; + } + } // the case of 4+*3 + + else if (xl < xh - 1 && yl < yh - 1) + { // the case of 3*3 + maze(xl + 1, yl + 1) = 1; + return; + } + else + { + ROS_INFO("finished generating maze with width %d , height %d", + xh - xl + 1, + yh - yl + 1); + return; + } +} + +void +Maps::recursizeDivisionMaze(Eigen::MatrixXi& maze) +{ + //! @todo all bugs here... + int sx = maze.rows(); + int sy = maze.cols(); + + int px, py; + + if (sx > 5) + px = (std::rand() % (sx - 3) + 1); + else + return; + + if (sy > 5) + py = (std::rand() % (sy - 3) + 1); + else + return; + + ROS_INFO("debug %d %d %d %d", sx, sy, px, py); + + int x1, x2, y1, y2; + + if (px != 1) + x1 = (std::rand() % (px - 1) + 1); + else + x1 = 1; + + if ((sx - px - 3) > 0) + x2 = (std::rand() % (sx - px - 3) + px + 1); + else + x2 = px + 1; + + if (py != 1) + y1 = (std::rand() % (py - 1) + 1); + else + y1 = 1; + + if ((sy - py - 3) > 0) + y2 = (std::rand() % (sy - py - 3) + py + 1); + else + y2 = py + 1; + ROS_INFO("%d %d %d %d", x1, x2, y1, y2); + + if (px != 1 && px != (sx - 2)) + { + for (int i = 1; i < (sy - 1); ++i) + { + if (i != y1 && i != y2) + maze(px, i) = 1; + } + } + if (py != 1 && py != (sy - 2)) + { + for (int i = 1; i < (sx - 1); ++i) + { + if (i != x1 && i != x2) + maze(i, py) = 1; + } + } + switch (std::rand() % 4) + { + case 0: + maze(x1, py) = 1; + break; + case 1: + maze(x2, py) = 1; + break; + case 2: + maze(px, y1) = 1; + break; + case 3: + maze(px, y2) = 1; + break; + } + + if (px > 2 && py > 2) + { + Eigen::MatrixXi sub = maze.block(0, 0, px + 1, py + 1); + recursizeDivisionMaze(sub); + maze.block(0, 0, px, py) = sub; + } + if (px > 2 && (sy - py - 1) > 2) + { + Eigen::MatrixXi sub = maze.block(0, py, px + 1, sy - py); + recursizeDivisionMaze(sub); + maze.block(0, py, px + 1, sy - py) = sub; + } + if (py > 2 && (sx - px - 1) > 2) + { + Eigen::MatrixXi sub = maze.block(px, 0, sx - px, py + 1); + recursizeDivisionMaze(sub); + maze.block(px, 0, sx - px, py + 1) = sub; + } + if ((sx - px - 1) > 2 && (sy - py - 1) > 2) + { + + Eigen::MatrixXi sub = maze.block(px, py, sy - px, sy - py); + + recursizeDivisionMaze(sub); + maze.block(px, py, sy - px, sy - py) = sub; + } +} + +void +Maps::maze2D() +{ + double width; + int type; + int addWallX; + int addWallY; + info.nh_private->param("road_width", width, 1.0); + info.nh_private->param("add_wall_x", addWallX, 0); + info.nh_private->param("add_wall_y", addWallY, 0); + info.nh_private->param("maze_type", type, 1); + + int mx = info.sizeX / (width * info.scale); + int my = info.sizeY / (width * info.scale); + + Eigen::MatrixXi maze(mx, my); + maze.setZero(); + + switch (type) + { + case 1: + recursiveDivision(0, maze.cols() - 1, 0, maze.rows() - 1, maze); + break; + } + + if (addWallX) + { + for (int i = 0; i < mx; ++i) + { + maze(i, 0) = 1; + maze(i, my - 1) = 1; + } + } + if (addWallY) + { + for (int i = 0; i < my; ++i) + { + maze(0, i) = 1; + maze(mx - 1, i) = 1; + } + } + + std::cout << maze << std::endl; + + for (int i = 0; i < mx; ++i) + { + for (int j = 0; j < my; ++j) + { + if (maze(i, j)) + { + for (int ii = 0; ii < width * info.scale; ++ii) + { + for (int jj = 0; jj < width * info.scale; ++jj) + { + for (int k = 0; k < info.sizeZ; ++k) + { + pcl::PointXYZ pt_random; + pt_random.x = + i * width + ii / info.scale - info.sizeX / (2.0 * info.scale); + pt_random.y = + j * width + jj / info.scale - info.sizeY / (2.0 * info.scale); + pt_random.z = k / info.scale; + info.cloud->points.push_back(pt_random); + } + } + } + } + } + } + info.cloud->width = info.cloud->points.size(); + info.cloud->height = 1; + info.cloud->is_dense = true; + pcl2ros(); +} + +Maps::BasicInfo +Maps::getInfo() const +{ + return info; +} + +void +Maps::setInfo(const BasicInfo& value) +{ + info = value; +} + +Maps::Maps() +{ +} + +void +Maps::generate(int type) +{ + switch (type) + { + default: + case 1: + perlin3D(); + break; + case 2: + randomMapGenerate(); + break; + case 3: + std::srand(info.seed); + maze2D(); + break; + case 4: // generating 3d maze + std::srand(info.seed); + Maze3DGen(); + break; + } +} + +pcl::PointXYZ +MazePoint::getPoint() +{ + return point; +} + +int +MazePoint::getPoint1() +{ + return point1; +} + +int +MazePoint::getPoint2() +{ + return point2; +} + +double +MazePoint::getDist1() +{ + return dist1; +} + +double +MazePoint::getDist2() +{ + return dist2; +} + +void +MazePoint::setPoint(pcl::PointXYZ p) +{ + point = p; +} + +void +MazePoint::setPoint1(int p) +{ + point1 = p; +} + +void +MazePoint::setPoint2(int p) +{ + point2 = p; +} + +void +MazePoint::setDist1(double set) +{ + dist1 = set; +} + +void +MazePoint::setDist2(double set) +{ + dist2 = set; +} + +void +Maps::Maze3DGen() +{ + // getting required info parameters from the given node + int numNodes; + double connectivity; + int nodeRad; + int roadRad; + + info.nh_private->param("numNodes", numNodes, 10); + info.nh_private->param("connectivity", connectivity, 0.5); + info.nh_private->param("nodeRad", nodeRad, 3); + info.nh_private->param("roadRad", roadRad, 2); + ROS_INFO("received parameters : numNodes: %d connectivity: " + "%f nodeRad: %d roadRad: %d", + numNodes, + connectivity, + nodeRad, + roadRad); + // generating random points + std::vector base; + + for (int i = 0; i < numNodes; i++) + { + double rx = std::rand() / RAND_MAX + + (std::rand() % info.sizeX) / info.scale - + info.sizeX / (2 * info.scale); + double ry = std::rand() / RAND_MAX + + (std::rand() % info.sizeY) / info.scale - + info.sizeY / (2 * info.scale); + double rz = std::rand() / RAND_MAX + + (std::rand() % info.sizeZ) / info.scale - + info.sizeZ / (2 * info.scale); + ROS_INFO("point: x: %f , y: %f , z: %f", rx, ry, rz); + + pcl::PointXYZ pt_random; + pt_random.x = rx; + pt_random.y = ry; + pt_random.z = rz; + base.push_back(pt_random); + } // generating random cores in the space + + for (int i = 0; i < info.sizeX; i++) + { + for (int j = 0; j < info.sizeY; j++) + { + for (int k = 0; k < info.sizeZ; k++) + { // for every scaled coordinate points + pcl::PointXYZ test; + test.x = i / info.scale - info.sizeX / (2 * info.scale); + test.y = j / info.scale - info.sizeY / (2 * info.scale); + test.z = k / info.scale - + info.sizeZ / + (2 * info.scale); // marking the corresponding point location + + MazePoint mp; + mp.setPoint(test); + mp.setPoint2(-1); + mp.setPoint1(-1); + mp.setDist1(10000.0); + mp.setDist2(100000.0); // setting super large starting values + for (int ii = 0; ii < numNodes; ii++) + { + double dist = + std::sqrt((base[ii].x - test.x) * (base[ii].x - test.x) + + (base[ii].y - test.y) * (base[ii].y - test.y) + + (base[ii].z - test.z) * (base[ii].z - test.z)); + if (dist < mp.getDist1()) + { + + mp.setDist2(mp.getDist1()); + mp.setDist1(dist); + + mp.setPoint2(mp.getPoint1()); + mp.setPoint1(ii); + } + else if (dist < mp.getDist2()) + { + mp.setDist2(dist); + mp.setPoint2(ii); + } // finding the distances to the nearest two cores + } + if (std::abs(mp.getDist2() - mp.getDist1()) < 1 / info.scale) + { // the tested location is on one of the middle planes + if ((mp.getPoint1() + mp.getPoint2()) > + int((1 - connectivity) * numNodes) && + (mp.getPoint1() + mp.getPoint2()) < + int((1 + connectivity) * numNodes)) + { // this is a holed wall + double judge = + std::sqrt((base[mp.getPoint1()].x - base[mp.getPoint2()].x) * + (base[mp.getPoint1()].x - base[mp.getPoint2()].x) + + (base[mp.getPoint1()].y - base[mp.getPoint2()].y) * + (base[mp.getPoint1()].y - base[mp.getPoint2()].y) + + (base[mp.getPoint1()].z - base[mp.getPoint2()].z) * + (base[mp.getPoint1()].z - base[mp.getPoint2()].z)); + if (mp.getDist1() + mp.getDist2() - judge >= + roadRad / (info.scale * 3)) + { + info.cloud->points.push_back(mp.getPoint()); + } + } + else + { + info.cloud->points.push_back(mp.getPoint()); + } + } + } + } + } + + info.cloud->width = info.cloud->points.size(); + info.cloud->height = 1; + ROS_INFO("the number of points before optimization is %d", info.cloud->width); + info.cloud->points.resize(info.cloud->width * info.cloud->height); + pcl2ros(); +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/mockamap.cpp b/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/mockamap.cpp new file mode 100644 index 0000000..9ff398b --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/mockamap.cpp @@ -0,0 +1,124 @@ +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "maps.hpp" + +void +optimizeMap(mocka::Maps::BasicInfo& in) +{ + std::vector* temp = new std::vector; + + pcl::KdTreeFLANN kdtree; + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + + cloud->width = in.cloud->width; + cloud->height = in.cloud->height; + cloud->points.resize(cloud->width * cloud->height); + + for (uint32_t i = 0; i < cloud->width; i++) + { + cloud->points[i].x = in.cloud->points[i].x; + cloud->points[i].y = in.cloud->points[i].y; + cloud->points[i].z = in.cloud->points[i].z; + } + + kdtree.setInputCloud(cloud); + double radius = 1.75 / in.scale; // 1.75 is the rounded up value of sqrt(3) + + for (uint32_t i = 0; i < cloud->width; i++) + { + std::vector pointIdxRadiusSearch; + std::vector pointRadiusSquaredDistance; + + if (kdtree.radiusSearch(cloud->points[i], radius, pointIdxRadiusSearch, + pointRadiusSquaredDistance) >= 27) + { + temp->push_back(i); + } + } + for (int i = temp->size() - 1; i >= 0; i--) + { + in.cloud->points.erase(in.cloud->points.begin() + + temp->at(i)); // erasing the enclosed points + } + in.cloud->width -= temp->size(); + + pcl::toROSMsg(*in.cloud, *in.output); + in.output->header.frame_id = "world"; + ROS_INFO("finish: number of points after optimization %d", in.cloud->width); + delete temp; + return; +} + +int +main(int argc, char** argv) +{ + ros::init(argc, argv, "mockamap"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + + ros::Publisher pcl_pub = + nh.advertise("mock_map", 1); + pcl::PointCloud cloud; + sensor_msgs::PointCloud2 output; + // Fill in the cloud data + + int seed; + + int sizeX; + int sizeY; + int sizeZ; + + double scale; + double update_freq; + + int type; + + nh_private.param("seed", seed, 4546); + nh_private.param("update_freq", update_freq, 1.0); + nh_private.param("resolution", scale, 0.38); + nh_private.param("x_length", sizeX, 100); + nh_private.param("y_length", sizeY, 100); + nh_private.param("z_length", sizeZ, 10); + + nh_private.param("type", type, 1); + + scale = 1 / scale; + sizeX = sizeX * scale; + sizeY = sizeY * scale; + sizeZ = sizeZ * scale; + + mocka::Maps::BasicInfo info; + info.nh_private = &nh_private; + info.sizeX = sizeX; + info.sizeY = sizeY; + info.sizeZ = sizeZ; + info.seed = seed; + info.scale = scale; + info.output = &output; + info.cloud = &cloud; + + mocka::Maps map; + map.setInfo(info); + map.generate(type); + + // optimizeMap(info); + + //! @note publish loop + ros::Rate loop_rate(update_freq); + while (ros::ok()) + { + pcl_pub.publish(output); + ros::spinOnce(); + loop_rate.sleep(); + } + return 0; +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/perlinnoise.cpp b/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/perlinnoise.cpp new file mode 100644 index 0000000..c5d96fe --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/mockamap/src/perlinnoise.cpp @@ -0,0 +1,123 @@ +#include "perlinnoise.hpp" + +#include +#include +#include +#include + +// THIS IS A DIRECT TRANSLATION TO C++11 FROM THE REFERENCE +// JAVA IMPLEMENTATION OF THE IMPROVED PERLIN FUNCTION (see +// http://mrl.nyu.edu/~perlin/noise/) +// THE ORIGINAL JAVA IMPLEMENTATION IS COPYRIGHT 2002 KEN PERLIN + +// I ADDED AN EXTRA METHOD THAT GENERATES A NEW PERMUTATION VECTOR (THIS IS NOT +// PRESENT IN THE ORIGINAL IMPLEMENTATION) + +// Initialize with the reference values for the permutation vector +PerlinNoise::PerlinNoise() +{ + + // Initialize the permutation vector with the reference values + p = { 151, 160, 137, 91, 90, 15, 131, 13, 201, 95, 96, 53, 194, 233, + 7, 225, 140, 36, 103, 30, 69, 142, 8, 99, 37, 240, 21, 10, + 23, 190, 6, 148, 247, 120, 234, 75, 0, 26, 197, 62, 94, 252, + 219, 203, 117, 35, 11, 32, 57, 177, 33, 88, 237, 149, 56, 87, + 174, 20, 125, 136, 171, 168, 68, 175, 74, 165, 71, 134, 139, 48, + 27, 166, 77, 146, 158, 231, 83, 111, 229, 122, 60, 211, 133, 230, + 220, 105, 92, 41, 55, 46, 245, 40, 244, 102, 143, 54, 65, 25, + 63, 161, 1, 216, 80, 73, 209, 76, 132, 187, 208, 89, 18, 169, + 200, 196, 135, 130, 116, 188, 159, 86, 164, 100, 109, 198, 173, 186, + 3, 64, 52, 217, 226, 250, 124, 123, 5, 202, 38, 147, 118, 126, + 255, 82, 85, 212, 207, 206, 59, 227, 47, 16, 58, 17, 182, 189, + 28, 42, 223, 183, 170, 213, 119, 248, 152, 2, 44, 154, 163, 70, + 221, 153, 101, 155, 167, 43, 172, 9, 129, 22, 39, 253, 19, 98, + 108, 110, 79, 113, 224, 232, 178, 185, 112, 104, 218, 246, 97, 228, + 251, 34, 242, 193, 238, 210, 144, 12, 191, 179, 162, 241, 81, 51, + 145, 235, 249, 14, 239, 107, 49, 192, 214, 31, 181, 199, 106, 157, + 184, 84, 204, 176, 115, 121, 50, 45, 127, 4, 150, 254, 138, 236, + 205, 93, 222, 114, 67, 29, 24, 72, 243, 141, 128, 195, 78, 66, + 215, 61, 156, 180 }; + // Duplicate the permutation vector + p.insert(p.end(), p.begin(), p.end()); +} + +// Generate a new permutation vector based on the value of seed +PerlinNoise::PerlinNoise(unsigned int seed) +{ + p.resize(256); + + // Fill p with values from 0 to 255 + std::iota(p.begin(), p.end(), 0); + + // Initialize a random engine with seed + std::default_random_engine engine(seed); + + // Suffle using the above random engine + std::shuffle(p.begin(), p.end(), engine); + + // Duplicate the permutation vector + p.insert(p.end(), p.begin(), p.end()); +} + +double +PerlinNoise::noise(double x, double y, double z) +{ + // Find the unit cube that contains the point + int X = (int)floor(x) & 255; + int Y = (int)floor(y) & 255; + int Z = (int)floor(z) & 255; + + // Find relative x, y,z of point in cube + x -= floor(x); + y -= floor(y); + z -= floor(z); + + // Compute fade curves for each of x, y, z + double u = fade(x); + double v = fade(y); + double w = fade(z); + + // Hash coordinates of the 8 cube corners + int A = p[X] + Y; + int AA = p[A] + Z; + int AB = p[A + 1] + Z; + int B = p[X + 1] + Y; + int BA = p[B] + Z; + int BB = p[B + 1] + Z; + + // Add blended results from 8 corners of cube + double res = + lerp(w, // + lerp(v, // + lerp(u, grad(p[AA], x, y, z), grad(p[BA], x - 1, y, z)), + lerp(u, grad(p[AB], x, y - 1, z), grad(p[BB], x - 1, y - 1, z))), + lerp(v, // + lerp(u, // + grad(p[AA + 1], x, y, z - 1), // + grad(p[BA + 1], x - 1, y, z - 1)), + lerp(u, // + grad(p[AB + 1], x, y - 1, z - 1), + grad(p[BB + 1], x - 1, y - 1, z - 1)))); + return (res + 1.0) / 2.0; +} + +double +PerlinNoise::fade(double t) +{ + return t * t * t * (t * (t * 6 - 15) + 10); +} + +double +PerlinNoise::lerp(double t, double a, double b) +{ + return a + t * (b - a); +} + +double +PerlinNoise::grad(int hash, double x, double y, double z) +{ + int h = hash & 15; + // Convert lower 4 bits of hash into 12 gradient directions + double u = h < 8 ? x : y, v = h < 4 ? y : h == 12 || h == 14 ? x : z; + return ((h & 1) == 0 ? u : -u) + ((h & 2) == 0 ? v : -v); +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/so3_control/CMakeLists.txt new file mode 100755 index 0000000..bb6b0c7 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/CMakeLists.txt @@ -0,0 +1,237 @@ +#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}) + +#list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +#find_package(Eigen3 REQUIRED) + +#include_directories(${EIGEN3_INCLUDE_DIR}) + +#rosbuild_add_library(SO3Control src/SO3Control.cpp) +#rosbuild_add_link_flags(SO3Control -Wl,--as-needed) + +#rosbuild_add_library(so3_control_nodelet src/so3_control_nodelet.cpp) +#target_link_libraries(so3_control_nodelet SO3Control) +#rosbuild_add_link_flags(so3_control_nodelet -Wl,--as-needed) + +#---------------------------------- +cmake_minimum_required(VERSION 2.8.3) +project(so3_control) + +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 + roscpp + nav_msgs + quadrotor_msgs + tf + nodelet + cmake_utils +) +set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) +## 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}) + +find_package(Eigen3 REQUIRED) + +include_directories(${EIGEN3_INCLUDE_DIR}) + + +## 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_library(SO3Control src/SO3Control.cpp) +add_library(so3_control_nodelet src/so3_control_nodelet.cpp) + +target_link_libraries(so3_control_nodelet + ${catkin_LIBRARIES} + SO3Control +) + + +add_executable(control_example src/control_example.cpp) +target_link_libraries(control_example +${catkin_LIBRARIES} +) + +## 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 +#) + +############# +## 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() diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/corrections_hummingbird.yaml b/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/corrections_hummingbird.yaml new file mode 100755 index 0000000..aeadaab --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/corrections_hummingbird.yaml @@ -0,0 +1,4 @@ +corrections: + z: 0.0 + r: 0.0 + p: 0.0 diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/corrections_pelican.yaml b/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/corrections_pelican.yaml new file mode 100755 index 0000000..aeadaab --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/corrections_pelican.yaml @@ -0,0 +1,4 @@ +corrections: + z: 0.0 + r: 0.0 + p: 0.0 diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/gains.yaml b/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/gains.yaml new file mode 100755 index 0000000..ffde85a --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/gains.yaml @@ -0,0 +1,6 @@ +# Gains for Laser-based Pelican +gains: + pos: {x: 5.0, y: 5.0, z: 15.0} + vel: {x: 5.0, y: 5.0, z: 5.0} + rot: {x: 3.5, y: 3.5, z: 1.0} + ang: {x: 0.4, y: 0.4, z: 0.1} diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/gains_hummingbird.yaml b/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/gains_hummingbird.yaml new file mode 100755 index 0000000..a1d016a --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/gains_hummingbird.yaml @@ -0,0 +1,6 @@ +# Vision Gain for Hummingbird +gains: + pos: {x: 2.0, y: 2.0, z: 3.5} + vel: {x: 1.8, y: 1.8, z: 2.0} + rot: {x: 1.0, y: 1.0, z: 0.3} + ang: {x: 0.07, y: 0.07, z: 0.02} diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/gains_pelican.yaml b/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/gains_pelican.yaml new file mode 100755 index 0000000..ffde85a --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/config/gains_pelican.yaml @@ -0,0 +1,6 @@ +# Gains for Laser-based Pelican +gains: + pos: {x: 5.0, y: 5.0, z: 15.0} + vel: {x: 5.0, y: 5.0, z: 5.0} + rot: {x: 3.5, y: 3.5, z: 1.0} + ang: {x: 0.4, y: 0.4, z: 0.1} diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/include/so3_control/SO3Control.h b/motion_planning/3d/ego_planner/uav_simulator/so3_control/include/so3_control/SO3Control.h new file mode 100755 index 0000000..afc2d95 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/include/so3_control/SO3Control.h @@ -0,0 +1,42 @@ +#ifndef __SO3_CONTROL_H__ +#define __SO3_CONTROL_H__ + +#include + +class SO3Control +{ +public: + SO3Control(); + + void setMass(const double mass); + void setGravity(const double g); + void setPosition(const Eigen::Vector3d& position); + void setVelocity(const Eigen::Vector3d& velocity); + void setAcc(const Eigen::Vector3d& acc); + + void calculateControl(const Eigen::Vector3d& des_pos, + const Eigen::Vector3d& des_vel, + const Eigen::Vector3d& des_acc, const double des_yaw, + const double des_yaw_dot, const Eigen::Vector3d& kx, + const Eigen::Vector3d& kv); + + const Eigen::Vector3d& getComputedForce(void); + const Eigen::Quaterniond& getComputedOrientation(void); + + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +private: + // Inputs for the controller + double mass_; + double g_; + Eigen::Vector3d pos_; + Eigen::Vector3d vel_; + Eigen::Vector3d acc_; + + // Outputs of the controller + Eigen::Vector3d force_; + Eigen::Quaterniond orientation_; +}; + +#endif diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/mainpage.dox b/motion_planning/3d/ego_planner/uav_simulator/so3_control/mainpage.dox new file mode 100755 index 0000000..101a5b1 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b so3_control + + + +--> + + +*/ diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/nodelet_plugin.xml b/motion_planning/3d/ego_planner/uav_simulator/so3_control/nodelet_plugin.xml new file mode 100755 index 0000000..af407f6 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/nodelet_plugin.xml @@ -0,0 +1,7 @@ + + + + so3_control + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/package.xml b/motion_planning/3d/ego_planner/uav_simulator/so3_control/package.xml new file mode 100755 index 0000000..c9273a4 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/package.xml @@ -0,0 +1,35 @@ + + 0.0.0 + so3_control + + + so3_control + + + Kartik Mohta + BSD + http://ros.org/wiki/so3_control + + + catkin + + roscpp + nav_msgs + quadrotor_msgs + tf + nodelet + cmake_utils + + roscpp + nav_msgs + quadrotor_msgs + tf + nodelet + cmake_utils + + + + + + + diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/src/SO3Control.cpp b/motion_planning/3d/ego_planner/uav_simulator/so3_control/src/SO3Control.cpp new file mode 100755 index 0000000..9d512b8 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/src/SO3Control.cpp @@ -0,0 +1,124 @@ +#include +#include + +#include + +SO3Control::SO3Control() + : mass_(0.5) + , g_(9.81) +{ + acc_.setZero(); +} + +void +SO3Control::setMass(const double mass) +{ + mass_ = mass; +} + +void +SO3Control::setGravity(const double g) +{ + g_ = g; +} + +void +SO3Control::setPosition(const Eigen::Vector3d& position) +{ + pos_ = position; +} + +void +SO3Control::setVelocity(const Eigen::Vector3d& velocity) +{ + vel_ = velocity; +} + +void +SO3Control::calculateControl(const Eigen::Vector3d& des_pos, + const Eigen::Vector3d& des_vel, + const Eigen::Vector3d& des_acc, + const double des_yaw, const double des_yaw_dot, + const Eigen::Vector3d& kx, + const Eigen::Vector3d& kv) +{ + // ROS_INFO("Error %lf %lf %lf", (des_pos - pos_).norm(), + // (des_vel - vel_).norm(), (des_acc - acc_).norm()); + + bool flag_use_pos = !(std::isnan(des_pos(0)) || std::isnan(des_pos(1)) || std::isnan(des_pos(2))); + bool flag_use_vel = !(std::isnan(des_vel(0)) || std::isnan(des_vel(1)) || std::isnan(des_vel(2))); + bool flag_use_acc = !(std::isnan(des_acc(0)) || std::isnan(des_acc(1)) || std::isnan(des_acc(2))); + + Eigen::Vector3d totalError(Eigen::Vector3d::Zero()); + if ( flag_use_pos ) totalError.noalias() += des_pos - pos_; + if ( flag_use_vel ) totalError.noalias() += des_vel - vel_; + if ( flag_use_acc ) totalError.noalias() += des_acc - acc_; + + Eigen::Vector3d ka(fabs(totalError[0]) > 3 ? 0 : (fabs(totalError[0]) * 0.2), + fabs(totalError[1]) > 3 ? 0 : (fabs(totalError[1]) * 0.2), + fabs(totalError[2]) > 3 ? 0 : (fabs(totalError[2]) * 0.2)); + + // std::cout << des_pos.transpose() << std::endl; + // std::cout << des_vel.transpose() << std::endl; + // std::cout << des_acc.transpose() << std::endl; + // std::cout << des_yaw << std::endl; + // std::cout << pos_.transpose() << std::endl; + // std::cout << vel_.transpose() << std::endl; + // std::cout << acc_.transpose() << std::endl; + + + force_ = mass_ * g_ * Eigen::Vector3d(0, 0, 1); + if ( flag_use_pos ) force_.noalias() += kx.asDiagonal() * (des_pos - pos_); + if ( flag_use_vel ) force_.noalias() += kv.asDiagonal() * (des_vel - vel_); + if ( flag_use_acc ) force_.noalias() += mass_ * ka.asDiagonal() * (des_acc - acc_) + mass_ * (des_acc); + + // Limit control angle to 45 degree + double theta = M_PI / 2; + double c = cos(theta); + Eigen::Vector3d f; + f.noalias() = force_ - mass_ * g_ * Eigen::Vector3d(0, 0, 1); + if (Eigen::Vector3d(0, 0, 1).dot(force_ / force_.norm()) < c) + { + double nf = f.norm(); + double A = c * c * nf * nf - f(2) * f(2); + double B = 2 * (c * c - 1) * f(2) * mass_ * g_; + double C = (c * c - 1) * mass_ * mass_ * g_ * g_; + double s = (-B + sqrt(B * B - 4 * A * C)) / (2 * A); + force_.noalias() = s * f + mass_ * g_ * Eigen::Vector3d(0, 0, 1); + } + // Limit control angle to 45 degree + + Eigen::Vector3d b1c, b2c, b3c; + Eigen::Vector3d b1d(cos(des_yaw), sin(des_yaw), 0); + + if (force_.norm() > 1e-6) + b3c.noalias() = force_.normalized(); + else + b3c.noalias() = Eigen::Vector3d(0, 0, 1); + + b2c.noalias() = b3c.cross(b1d).normalized(); + b1c.noalias() = b2c.cross(b3c).normalized(); + + Eigen::Matrix3d R; + R << b1c, b2c, b3c; + + orientation_ = Eigen::Quaterniond(R); +} + +const Eigen::Vector3d& +SO3Control::getComputedForce(void) +{ + return force_; +} + +const Eigen::Quaterniond& +SO3Control::getComputedOrientation(void) +{ + return orientation_; +} + +void +SO3Control::setAcc(const Eigen::Vector3d& acc) +{ + acc_ = acc; +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/src/control_example.cpp b/motion_planning/3d/ego_planner/uav_simulator/so3_control/src/control_example.cpp new file mode 100644 index 0000000..15a1738 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/src/control_example.cpp @@ -0,0 +1,78 @@ +#include +#include +#include + +int main(int argc, char **argv) +{ + + ros::init(argc, argv, "quad_sim_example"); + ros::NodeHandle nh("~"); + + ros::Publisher cmd_pub = nh.advertise("/position_cmd", 10); + + ros::Duration(2.0).sleep(); + + while (ros::ok()) + { + + /*** example 1: position control ***/ + std::cout << "\033[42m" + << "Position Control to (2,0,1) meters" + << "\033[0m" << std::endl; + for (int i = 0; i < 500; i++) + { + quadrotor_msgs::PositionCommand cmd; + cmd.position.x = 2.0; + cmd.position.y = 0.0; + cmd.position.z = 1.0; + cmd_pub.publish(cmd); + + ros::Duration(0.01).sleep(); + ros::spinOnce(); + } + + /*** example 1: position control ***/ + std::cout << "\033[42m" + << "Velocity Control to (-1,0,0) meters/second" + << "\033[0m" << std::endl; + for (int i = 0; i < 500; i++) + { + quadrotor_msgs::PositionCommand cmd; + cmd.position.x = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan + cmd.position.y = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan + cmd.position.z = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan + cmd.velocity.x = -1.0; + cmd.velocity.y = 0.0; + cmd.velocity.z = 0.0; + cmd_pub.publish(cmd); + + ros::Duration(0.01).sleep(); + ros::spinOnce(); + } + + /*** example 1: accelleration control ***/ + std::cout << "\033[42m" + << "Accelleration Control to (1,0,0) meters/second^2" + << "\033[0m" << std::endl; + for (int i = 0; i < 500; i++) + { + quadrotor_msgs::PositionCommand cmd; + cmd.position.x = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan + cmd.position.y = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan + cmd.position.z = std::numeric_limits::quiet_NaN(); // lower-order commands must be disabled by nan + cmd.velocity.x = std::numeric_limits::quiet_NaN(); + cmd.velocity.y = std::numeric_limits::quiet_NaN(); + cmd.velocity.z = std::numeric_limits::quiet_NaN(); + cmd.acceleration.x = 1.0; + cmd.acceleration.y = 0.0; + cmd.acceleration.z = 0.0; + cmd_pub.publish(cmd); + + ros::Duration(0.01).sleep(); + ros::spinOnce(); + } + + } + + return 0; +} diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_control/src/so3_control_nodelet.cpp b/motion_planning/3d/ego_planner/uav_simulator/so3_control/src/so3_control_nodelet.cpp new file mode 100755 index 0000000..8135ed2 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_control/src/so3_control_nodelet.cpp @@ -0,0 +1,245 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SO3ControlNodelet : public nodelet::Nodelet +{ +public: + SO3ControlNodelet() + : position_cmd_updated_(false) + , position_cmd_init_(false) + , des_yaw_(0) + , des_yaw_dot_(0) + , current_yaw_(0) + , enable_motors_(true) + , // FIXME + use_external_yaw_(false) + { + } + + void onInit(void); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +private: + void publishSO3Command(void); + void position_cmd_callback( + const quadrotor_msgs::PositionCommand::ConstPtr& cmd); + void odom_callback(const nav_msgs::Odometry::ConstPtr& odom); + void enable_motors_callback(const std_msgs::Bool::ConstPtr& msg); + void corrections_callback(const quadrotor_msgs::Corrections::ConstPtr& msg); + void imu_callback(const sensor_msgs::Imu& imu); + + SO3Control controller_; + ros::Publisher so3_command_pub_; + ros::Subscriber odom_sub_; + ros::Subscriber position_cmd_sub_; + ros::Subscriber enable_motors_sub_; + ros::Subscriber corrections_sub_; + ros::Subscriber imu_sub_; + + bool position_cmd_updated_, position_cmd_init_; + std::string frame_id_; + + Eigen::Vector3d des_pos_, des_vel_, des_acc_, kx_, kv_; + double des_yaw_, des_yaw_dot_; + double current_yaw_; + bool enable_motors_; + bool use_external_yaw_; + double kR_[3], kOm_[3], corrections_[3]; + double init_x_, init_y_, init_z_; +}; + +void +SO3ControlNodelet::publishSO3Command(void) +{ + controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_, + des_yaw_dot_, kx_, kv_); + + const Eigen::Vector3d& force = controller_.getComputedForce(); + const Eigen::Quaterniond& orientation = controller_.getComputedOrientation(); + + quadrotor_msgs::SO3Command::Ptr so3_command( + new quadrotor_msgs::SO3Command); //! @note memory leak? + so3_command->header.stamp = ros::Time::now(); + so3_command->header.frame_id = frame_id_; + so3_command->force.x = force(0); + so3_command->force.y = force(1); + so3_command->force.z = force(2); + so3_command->orientation.x = orientation.x(); + so3_command->orientation.y = orientation.y(); + so3_command->orientation.z = orientation.z(); + so3_command->orientation.w = orientation.w(); + for (int i = 0; i < 3; i++) + { + so3_command->kR[i] = kR_[i]; + so3_command->kOm[i] = kOm_[i]; + } + so3_command->aux.current_yaw = current_yaw_; + so3_command->aux.kf_correction = corrections_[0]; + so3_command->aux.angle_corrections[0] = corrections_[1]; + so3_command->aux.angle_corrections[1] = corrections_[2]; + so3_command->aux.enable_motors = enable_motors_; + so3_command->aux.use_external_yaw = use_external_yaw_; + so3_command_pub_.publish(so3_command); +} + +void +SO3ControlNodelet::position_cmd_callback( + const quadrotor_msgs::PositionCommand::ConstPtr& cmd) +{ + des_pos_ = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z); + des_vel_ = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); + des_acc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y, + cmd->acceleration.z); + + if ( cmd->kx[0] > 1e-5 || cmd->kx[1] > 1e-5 || cmd->kx[2] > 1e-5 ) + { + kx_ = Eigen::Vector3d(cmd->kx[0], cmd->kx[1], cmd->kx[2]); + } + if ( cmd->kv[0] > 1e-5 || cmd->kv[1] > 1e-5 || cmd->kv[2] > 1e-5 ) + { + kv_ = Eigen::Vector3d(cmd->kv[0], cmd->kv[1], cmd->kv[2]); + } + + des_yaw_ = cmd->yaw; + des_yaw_dot_ = cmd->yaw_dot; + position_cmd_updated_ = true; + position_cmd_init_ = true; + + publishSO3Command(); +} + +void +SO3ControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr& odom) +{ + const Eigen::Vector3d position(odom->pose.pose.position.x, + odom->pose.pose.position.y, + odom->pose.pose.position.z); + const Eigen::Vector3d velocity(odom->twist.twist.linear.x, + odom->twist.twist.linear.y, + odom->twist.twist.linear.z); + + current_yaw_ = tf::getYaw(odom->pose.pose.orientation); + + controller_.setPosition(position); + controller_.setVelocity(velocity); + + if (position_cmd_init_) + { + // We set position_cmd_updated_ = false and expect that the + // position_cmd_callback would set it to true since typically a position_cmd + // message would follow an odom message. If not, the position_cmd_callback + // hasn't been called and we publish the so3 command ourselves + // TODO: Fallback to hover if position_cmd hasn't been received for some + // time + if (!position_cmd_updated_) + publishSO3Command(); + position_cmd_updated_ = false; + } + else if ( init_z_ > -9999.0 ) + { + des_pos_ = Eigen::Vector3d(init_x_, init_y_, init_z_); + des_vel_ = Eigen::Vector3d(0,0,0); + des_acc_ = Eigen::Vector3d(0,0,0); + publishSO3Command(); + } + +} + +void +SO3ControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr& msg) +{ + if (msg->data) + ROS_INFO("Enabling motors"); + else + ROS_INFO("Disabling motors"); + + enable_motors_ = msg->data; +} + +void +SO3ControlNodelet::corrections_callback( + const quadrotor_msgs::Corrections::ConstPtr& msg) +{ + corrections_[0] = msg->kf_correction; + corrections_[1] = msg->angle_corrections[0]; + corrections_[2] = msg->angle_corrections[1]; +} + +void +SO3ControlNodelet::imu_callback(const sensor_msgs::Imu& imu) +{ + const Eigen::Vector3d acc(imu.linear_acceleration.x, + imu.linear_acceleration.y, + imu.linear_acceleration.z); + controller_.setAcc(acc); +} + +void +SO3ControlNodelet::onInit(void) +{ + ros::NodeHandle n(getPrivateNodeHandle()); + + std::string quadrotor_name; + n.param("quadrotor_name", quadrotor_name, std::string("quadrotor")); + frame_id_ = "/" + quadrotor_name; + + double mass; + n.param("mass", mass, 0.5); + controller_.setMass(mass); + + n.param("use_external_yaw", use_external_yaw_, true); + + n.param("gains/rot/x", kR_[0], 1.5); + n.param("gains/rot/y", kR_[1], 1.5); + n.param("gains/rot/z", kR_[2], 1.0); + n.param("gains/ang/x", kOm_[0], 0.13); + n.param("gains/ang/y", kOm_[1], 0.13); + n.param("gains/ang/z", kOm_[2], 0.1); + n.param("gains/kx/x", kx_[0], 5.7); + n.param("gains/kx/y", kx_[1], 5.7); + n.param("gains/kx/z", kx_[2], 6.2); + n.param("gains/kv/x", kv_[0], 3.4); + n.param("gains/kv/y", kv_[1], 3.4); + n.param("gains/kv/z", kv_[2], 4.0); + + n.param("corrections/z", corrections_[0], 0.0); + n.param("corrections/r", corrections_[1], 0.0); + n.param("corrections/p", corrections_[2], 0.0); + + n.param("so3_control/init_state_x", init_x_, 0.0); + n.param("so3_control/init_state_y", init_y_, 0.0); + n.param("so3_control/init_state_z", init_z_, -10000.0); + + so3_command_pub_ = n.advertise("so3_cmd", 10); + + odom_sub_ = n.subscribe("odom", 10, &SO3ControlNodelet::odom_callback, this, + ros::TransportHints().tcpNoDelay()); + position_cmd_sub_ = + n.subscribe("position_cmd", 10, &SO3ControlNodelet::position_cmd_callback, + this, ros::TransportHints().tcpNoDelay()); + + enable_motors_sub_ = + n.subscribe("motors", 2, &SO3ControlNodelet::enable_motors_callback, this, + ros::TransportHints().tcpNoDelay()); + corrections_sub_ = + n.subscribe("corrections", 10, &SO3ControlNodelet::corrections_callback, + this, ros::TransportHints().tcpNoDelay()); + + imu_sub_ = n.subscribe("imu", 10, &SO3ControlNodelet::imu_callback, this, + ros::TransportHints().tcpNoDelay()); +} + +#include +//PLUGINLIB_DECLARE_CLASS(so3_control, SO3ControlNodelet, SO3ControlNodelet, +// nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(SO3ControlNodelet, nodelet::Nodelet); diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/CMakeLists.txt b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/CMakeLists.txt new file mode 100755 index 0000000..6fd4e2f --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 2.8.3) +project(so3_quadrotor_simulator) + +add_compile_options(-std=c++11) + +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 + roscpp + quadrotor_msgs + uav_utils + cmake_utils +) + +########### +## Build ## +########### + +find_package(Eigen3 REQUIRED) + +include_directories(${EIGEN3_INCLUDE_DIR}) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/ode) +## +find_package(Armadillo REQUIRED) +include_directories(${ARMADILLO_INCLUDE_DIRS}) + +catkin_package( + INCLUDE_DIRS include +# LIBRARIES irobot_msgs + CATKIN_DEPENDS roscpp quadrotor_msgs uav_utils + DEPENDS Eigen3 system_lib +) + +include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_library(quadrotor_dynamics src/dynamics/Quadrotor.cpp) + +## Declare a cpp executable +#add_executable(odom_visualization src/odom_visualization.cpp) +add_executable(quadrotor_simulator_so3 + src/quadrotor_simulator_so3.cpp) + +target_link_libraries(quadrotor_simulator_so3 + ${catkin_LIBRARIES} + quadrotor_dynamics +) diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/config/rviz.rviz b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/config/rviz.rviz new file mode 100644 index 0000000..0a8060b --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/config/rviz.rviz @@ -0,0 +1,212 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /UAV1 + Splitter Ratio: 0.5 + Tree Height: 703 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + - /3D Nav Goal1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 138; 138; 138 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization/robot + Name: UAV + Namespaces: + mesh: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 255 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /odom_visualization_1/path + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.100000001 + Class: rviz/Pose + Color: 0; 255; 0 + Enabled: true + Head Length: 0.300000012 + Head Radius: 0.100000001 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.0500000007 + Shape: Arrow + Topic: /odom_visualization_1/pose + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /odom_visualization_1/velocity + Name: Velocity + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 255 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /CircleNode_1/path + Unreliable: false + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.100000001 + Reference Frame: + Value: true + Enabled: true + Global Options: + Background Color: 89; 89; 89 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + - Class: rviz_plugins/GameLikeInput + RangeX_max: 4 + RangeX_min: -4 + RangeY_max: 2.5 + RangeY_min: -2.5 + RangeZ_max: 1.70000005 + RangeZ_min: 0.800000012 + TopicPoint: point_list + TopicSelect: select_list + TopicSwarm: swarm + - Class: rviz_plugins/Goal3DTool + Topic: goal + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 14.550209 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.054024078 + Y: -0.301004827 + Z: 2.69711113 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.759796858 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.71485233 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 984 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000034e000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005400000003efc0100000002fb0000000800540069006d00650100000000000005400000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003d00000034e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1344 + X: 565 + Y: 24 diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/CHANGELOG b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/CHANGELOG new file mode 100755 index 0000000..b1537eb --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/CHANGELOG @@ -0,0 +1,9 @@ +odeint 2.1 + +* versioning system +* generation functions +* bugfixing + +odeint 2.2 (still running) + +* removing same_size and resize from state_wrapper into separate functions diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/Jamroot b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/Jamroot new file mode 100755 index 0000000..db8bfbc --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/Jamroot @@ -0,0 +1,44 @@ +# Copyright 2009 Karsten Ahnert and Mario Mulansky. +# Distributed under the Boost Software License, Version 1.0. (See +# accompanying file LICENSE_1_0.txt or copy at +# http://www.boost.org/LICENSE_1_0.txt) + +import os ; +import modules ; +import path ; + +path-constant BOOST_ROOT : [ os.environ BOOST_ROOT ] ; + +project + : requirements + $(BOOST_ROOT) + clang:-Wno-unused-variable + ; + +# tests, regression tests and examples +build-project libs/numeric/odeint/test ; +build-project libs/numeric/odeint/examples ; + + +# additional tests with external libraries : +# build-project libs/numeric/odeint/test_external/gmp ; +# build-project libs/numeric/odeint/test_external/mkl ; +# build-project libs/numeric/odeint/test_external/gsl ; + + +# docs: +# build-project libs/numeric/odeint/doc ; + + + + + + +###### The following is copied from another sandbox project ##### +###### to get the quickbook and boostbook working ... ##### + +# local boost-root = [ modules.peek : BOOST_ROOT ] ; +# local explore-header-include = $(top)/../.. ; +# use-project /boost/regex : $(boost-root)/libs/regex/build ; + +################################################################## diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/README b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/README new file mode 100755 index 0000000..f34601f --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/README @@ -0,0 +1 @@ +odeint is a highly flexible library for solving ordinary differential equations. diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint.hpp new file mode 100755 index 0000000..3200fbe --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint.hpp @@ -0,0 +1,75 @@ +/* + [auto_generated] + boost/numeric/odeint.hpp + + [begin_description] + Forward include for odeint. Includes nearly everything. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_HPP_INCLUDED + +#include +#include + +// start with ublas wrapper because we need its specializations before including state_wrapper.hpp +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +#ifndef __CUDACC__ +/* Bulirsch Stoer with Dense Output does not compile with nvcc + * because of the binomial library used there which relies on unsupported SSE functions + */ +#include +#endif + +#include +#include + +#include + +#include +#include +#include +#include + +/* + * Including this algebra slows down the compilation time + */ +// #include +#include + +#include +#include +#include +#include +#include + +#include + +#include + + +#endif // BOOST_NUMERIC_ODEINT_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/array_algebra.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/array_algebra.hpp new file mode 100755 index 0000000..096e427 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/array_algebra.hpp @@ -0,0 +1,265 @@ +/* + [auto_generated] + boost/numeric/odeint/algebra/array_algebra.hpp + + [begin_description] + Algebra for boost::array. Highly specialized for odeint. Const arguments are introduce to work with odeint. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_ARRAY_ALGEBRA_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_ALGEBRA_ARRAY_ALGEBRA_HPP_INCLUDED + +#include + +namespace boost { +namespace numeric { +namespace odeint { + +struct array_algebra +{ + template< typename T , size_t dim , class Op > + static void for_each1( boost::array< T , dim > &s1 , Op op ) + { + for( size_t i=0 ; i + static void for_each2( boost::array< T1 , dim > &s1 , + const boost::array< T2 , dim > &s2 , Op op ) + { + for( size_t i=0 ; i + static void for_each3( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , Op op ) + { + for( size_t i=0 ; i + static void for_each3( boost::array< T , dim > &s1 , + boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , Op op ) + { + for( size_t i=0 ; i + static void for_each4( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , Op op ) + { + for( size_t i=0 ; i + static void for_each5( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , + const boost::array< T , dim > &s5 , Op op ) + { + for( size_t i=0 ; i + static void for_each6( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , + const boost::array< T , dim > &s5 , + const boost::array< T , dim > &s6 , Op op ) + { + for( size_t i=0 ; i + static void for_each7( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , + const boost::array< T , dim > &s5 , + const boost::array< T , dim > &s6 , + const boost::array< T , dim > &s7 , Op op ) + { + for( size_t i=0 ; i + static void for_each8( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , + const boost::array< T , dim > &s5 , + const boost::array< T , dim > &s6 , + const boost::array< T , dim > &s7 , + const boost::array< T , dim > &s8 , Op op ) + { + for( size_t i=0 ; i + static void for_each9( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , + const boost::array< T , dim > &s5 , + const boost::array< T , dim > &s6 , + const boost::array< T , dim > &s7 , + const boost::array< T , dim > &s8 , + const boost::array< T , dim > &s9 , Op op ) + { + for( size_t i=0 ; i + static void for_each10( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , + const boost::array< T , dim > &s5 , + const boost::array< T , dim > &s6 , + const boost::array< T , dim > &s7 , + const boost::array< T , dim > &s8 , + const boost::array< T , dim > &s9 , + const boost::array< T , dim > &s10 , Op op ) + { + for( size_t i=0 ; i + static void for_each11( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , + const boost::array< T , dim > &s5 , + const boost::array< T , dim > &s6 , + const boost::array< T , dim > &s7 , + const boost::array< T , dim > &s8 , + const boost::array< T , dim > &s9 , + const boost::array< T , dim > &s10 , + const boost::array< T , dim > &s11 , Op op ) + { + for( size_t i=0 ; i + static void for_each12( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , + const boost::array< T , dim > &s5 , + const boost::array< T , dim > &s6 , + const boost::array< T , dim > &s7 , + const boost::array< T , dim > &s8 , + const boost::array< T , dim > &s9 , + const boost::array< T , dim > &s10 , + const boost::array< T , dim > &s11 , + const boost::array< T , dim > &s12 , Op op ) + { + for( size_t i=0 ; i + static void for_each13( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , + const boost::array< T , dim > &s5 , + const boost::array< T , dim > &s6 , + const boost::array< T , dim > &s7 , + const boost::array< T , dim > &s8 , + const boost::array< T , dim > &s9 , + const boost::array< T , dim > &s10 , + const boost::array< T , dim > &s11 , + const boost::array< T , dim > &s12 , + const boost::array< T , dim > &s13 , Op op ) + { + for( size_t i=0 ; i + static void for_each14( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , + const boost::array< T , dim > &s5 , + const boost::array< T , dim > &s6 , + const boost::array< T , dim > &s7 , + const boost::array< T , dim > &s8 , + const boost::array< T , dim > &s9 , + const boost::array< T , dim > &s10 , + const boost::array< T , dim > &s11 , + const boost::array< T , dim > &s12 , + const boost::array< T , dim > &s13 , + const boost::array< T , dim > &s14 , Op op ) + { + for( size_t i=0 ; i + static void for_each15( boost::array< T , dim > &s1 , + const boost::array< T , dim > &s2 , + const boost::array< T , dim > &s3 , + const boost::array< T , dim > &s4 , + const boost::array< T , dim > &s5 , + const boost::array< T , dim > &s6 , + const boost::array< T , dim > &s7 , + const boost::array< T , dim > &s8 , + const boost::array< T , dim > &s9 , + const boost::array< T , dim > &s10 , + const boost::array< T , dim > &s11 , + const boost::array< T , dim > &s12 , + const boost::array< T , dim > &s13 , + const boost::array< T , dim > &s14 , + const boost::array< T , dim > &s15 , Op op ) + { + for( size_t i=0 ; i + static Value reduce( const boost::array< T , dim > &s , Red red , Value init) + { + for( size_t i=0 ; i ... + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DEFAULT_OPERATIONS_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_ALGEBRA_DEFAULT_OPERATIONS_HPP_INCLUDED + +#include + +#include +#include + +#include + + +namespace boost { +namespace numeric { +namespace odeint { + + + +/* + * Notes: + * + * * the results structs are needed in order to work with fusion_algebra + */ +struct default_operations +{ + + template< class Fac1 = double > + struct scale + { + const Fac1 m_alpha1; + + scale( Fac1 alpha1 ) : m_alpha1( alpha1 ) { } + + template< class T1 > + void operator()( T1 &t1 ) const + { + t1 *= m_alpha1; + } + + typedef void result_type; + }; + + template< class Fac1 = double > + struct scale_sum1 + { + const Fac1 m_alpha1; + + scale_sum1( Fac1 alpha1 ) : m_alpha1( alpha1 ) { } + + template< class T1 , class T2 > + void operator()( T1 &t1 , const T2 &t2 ) const + { + t1 = m_alpha1 * t2; + } + + typedef void result_type; + }; + + + template< class Fac1 = double , class Fac2 = Fac1 > + struct scale_sum2 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + + scale_sum2( Fac1 alpha1 , Fac2 alpha2 ) : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { } + + template< class T1 , class T2 , class T3 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3; + } + + typedef void result_type; + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 > + struct scale_sum3 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + + scale_sum3( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) { } + + template< class T1 , class T2 , class T3 , class T4 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 ) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4; + } + + typedef void result_type; + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 > + struct scale_sum4 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + + scale_sum4( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5; + } + + typedef void result_type; + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 > + struct scale_sum5 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + + scale_sum5( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , Fac5 alpha5 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6; + } + + typedef void result_type; + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 > + struct scale_sum6 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + + scale_sum6( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , Fac5 alpha5 , Fac6 alpha6 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ){ } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 ,const T7 &t7) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7; + } + + typedef void result_type; + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 > + struct scale_sum7 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + + scale_sum7( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , + Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 ) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8; + } + + typedef void result_type; + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 > + struct scale_sum8 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + + scale_sum8( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , + Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 ) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9; + } + + typedef void result_type; + }; + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 > + struct scale_sum9 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + + scale_sum9( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , + Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 ) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10; + } + + typedef void result_type; + }; + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 > + struct scale_sum10 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + const Fac10 m_alpha10; + + scale_sum10( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , + Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 , Fac10 alpha10 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 ) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11; + } + + typedef void result_type; + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 > + struct scale_sum11 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + const Fac10 m_alpha10; + const Fac11 m_alpha11; + + scale_sum11( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , + Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 , + Fac10 alpha10 , Fac11 alpha11 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 ) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12; + } + + typedef void result_type; + }; + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 , class Fac12 = Fac11 > + struct scale_sum12 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + const Fac10 m_alpha10; + const Fac11 m_alpha11; + const Fac12 m_alpha12; + + scale_sum12( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , + Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 , + Fac10 alpha10 , Fac11 alpha11 , Fac12 alpha12 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) , m_alpha12( alpha12 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 , class T13 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 , const T13 &t13 ) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + m_alpha12 * t13; + } + + typedef void result_type; + }; + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 , class Fac12 = Fac11 , class Fac13 = Fac12 > + struct scale_sum13 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + const Fac10 m_alpha10; + const Fac11 m_alpha11; + const Fac12 m_alpha12; + const Fac13 m_alpha13; + + scale_sum13( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , + Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 , + Fac10 alpha10 , Fac11 alpha11 , Fac12 alpha12 , Fac13 alpha13 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) , m_alpha12( alpha12 ) , m_alpha13( alpha13 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 , class T13 , class T14 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 , const T13 &t13 , const T14 &t14 ) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + m_alpha12 * t13 + m_alpha13 * t14; + } + + typedef void result_type; + }; + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 , class Fac12 = Fac11 , class Fac13 = Fac12 , class Fac14 = Fac13 > + struct scale_sum14 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + const Fac10 m_alpha10; + const Fac11 m_alpha11; + const Fac12 m_alpha12; + const Fac13 m_alpha13; + const Fac14 m_alpha14; + + scale_sum14( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , + Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 , + Fac10 alpha10 , Fac11 alpha11 , Fac12 alpha12 , Fac13 alpha13 , Fac14 alpha14 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) , m_alpha12( alpha12 ) , m_alpha13( alpha13 ) , m_alpha14( alpha14 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 , class T13 , class T14 , class T15 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 , const T13 &t13 , const T14 &t14 , const T15 &t15 ) const + { + t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + m_alpha12 * t13 + m_alpha13 * t14 + m_alpha14 * t15; + } + + typedef void result_type; + }; + + template< class Fac1 = double , class Fac2 = Fac1 > + struct scale_sum_swap2 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + + scale_sum_swap2( Fac1 alpha1 , Fac2 alpha2 ) : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { } + + template< class T1 , class T2 , class T3 > + void operator()( T1 &t1 , T2 &t2 , const T3 &t3) const + { + const T1 tmp( t1 ); + t1 = m_alpha1 * t2 + m_alpha2 * t3; + t2 = tmp; + } + + typedef void result_type; + }; + + /* + * for usage in for_each2 + * + * Works with boost::units by eliminating the unit + */ + template< class Fac1 = double > + struct rel_error + { + const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt; + + rel_error( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt ) + : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt ) { } + + + template< class T1 , class T2 , class T3 > + void operator()( T3 &t3 , const T1 &t1 , const T2 &t2 ) const + { + using std::abs; + set_unit_value( t3 , abs( get_unit_value( t3 ) ) / ( m_eps_abs + m_eps_rel * ( m_a_x * abs( get_unit_value( t1 ) ) + m_a_dxdt * abs( get_unit_value( t2 ) ) ) ) ); + } + + typedef void result_type; + }; + + + /* + * for usage in for_each3 + * + * used in the controller for the rosenbrock4 method + * + * Works with boost::units by eliminating the unit + */ + template< class Fac1 = double > + struct default_rel_error + { + const Fac1 m_eps_abs , m_eps_rel ; + + default_rel_error( Fac1 eps_abs , Fac1 eps_rel ) + : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) { } + + + /* + * xerr = xerr / ( eps_abs + eps_rel * max( x , x_old ) ) + */ + template< class T1 , class T2 , class T3 > + void operator()( T3 &t3 , const T1 &t1 , const T2 &t2 ) const + { + BOOST_USING_STD_MAX(); + using std::abs; + Fac1 x1 = abs( get_unit_value( t1 ) ) , x2 = abs( get_unit_value( t2 ) ); + set_unit_value( t3 , abs( get_unit_value( t3 ) ) / ( m_eps_abs + m_eps_rel * max BOOST_PREVENT_MACRO_SUBSTITUTION ( x1 , x2 ) ) ); + } + + typedef void result_type; + }; + + + + /* + * for usage in reduce + */ + + template< class Value > + struct maximum + { + template< class Fac1 , class Fac2 > + Value operator()( Fac1 t1 , const Fac2 t2 ) const + { + using std::abs; + Value a1 = abs( get_unit_value( t1 ) ) , a2 = abs( get_unit_value( t2 ) ); + return ( a1 < a2 ) ? a2 : a1 ; + } + + typedef Value result_type; + }; + + + + + + template< class Fac1 = double > + struct rel_error_max + { + const Fac1 m_eps_abs , m_eps_rel; + + rel_error_max( Fac1 eps_abs , Fac1 eps_rel ) + : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) + { } + + template< class Res , class T1 , class T2 , class T3 > + Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &x_err ) + { + BOOST_USING_STD_MAX(); + using std::abs; + Res tmp = abs( get_unit_value( x_err ) ) / ( m_eps_abs + m_eps_rel * max BOOST_PREVENT_MACRO_SUBSTITUTION ( abs( x_old ) , abs( x ) ) ); + return max BOOST_PREVENT_MACRO_SUBSTITUTION ( r , tmp ); + } + }; + + + template< class Fac1 = double > + struct rel_error_max2 + { + const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt; + + rel_error_max2( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt ) + : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt ) + { } + + template< class Res , class T1 , class T2 , class T3 , class T4 > + Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &dxdt_old , const T4 &x_err ) + { + BOOST_USING_STD_MAX(); + using std::abs; + Res tmp = abs( get_unit_value( x_err ) ) / + ( m_eps_abs + m_eps_rel * ( m_a_x * abs( get_unit_value( x_old ) ) + m_a_dxdt * abs( get_unit_value( dxdt_old ) ) ) ); + return max BOOST_PREVENT_MACRO_SUBSTITUTION ( r , tmp ); + } + }; + + + + + template< class Fac1 = double > + struct rel_error_l2 + { + const Fac1 m_eps_abs , m_eps_rel; + + rel_error_l2( Fac1 eps_abs , Fac1 eps_rel ) + : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) + { } + + template< class Res , class T1 , class T2 , class T3 > + Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &x_err ) + { + BOOST_USING_STD_MAX(); + using std::abs; + Res tmp = abs( get_unit_value( x_err ) ) / ( m_eps_abs + m_eps_rel * max BOOST_PREVENT_MACRO_SUBSTITUTION ( abs( x_old ) , abs( x ) ) ); + return r + tmp * tmp; + } + }; + + + + + template< class Fac1 = double > + struct rel_error_l2_2 + { + const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt; + + rel_error_l2_2( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt ) + : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt ) + { } + + template< class Res , class T1 , class T2 , class T3 , class T4 > + Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &dxdt_old , const T4 &x_err ) + { + using std::abs; + Res tmp = abs( get_unit_value( x_err ) ) / + ( m_eps_abs + m_eps_rel * ( m_a_x * abs( get_unit_value( x_old ) ) + m_a_dxdt * abs( get_unit_value( dxdt_old ) ) ) ); + return r + tmp * tmp; + } + }; + + + + + + +}; + + +} // odeint +} // numeric +} // boost + + +#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DEFAULT_OPERATIONS_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/for_each.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/for_each.hpp new file mode 100755 index 0000000..8693e84 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/for_each.hpp @@ -0,0 +1,165 @@ +/* + [auto_generated] + boost/numeric/odeint/algebra/detail/for_each.hpp + + [begin_description] + Default for_each implementations. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_FOR_EACH_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_FOR_EACH_HPP_INCLUDED + +namespace boost { +namespace numeric { +namespace odeint { +namespace detail { + + + template< class Iterator1 , class Operation > + inline void for_each1( Iterator1 first1 , Iterator1 last1 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ ); + } + + + template< class Iterator1 , class Iterator2 , class Operation > + inline void for_each2( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ ); + } + + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Operation > + inline void for_each3( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ ); + } + + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Operation > + inline void for_each4( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, Iterator4 first4, Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ ); + } + + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Operation > + inline void for_each5( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, + Iterator4 first4, Iterator5 first5, Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ ); + } + + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Operation > + inline void for_each6( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, + Iterator4 first4, Iterator5 first5, Iterator6 first6 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ ); + } + + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Operation > + inline void for_each7( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, + Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ ); + } + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Operation > + inline void for_each8( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, + Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ ); + } + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Operation > + inline void for_each9( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, + Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , + Iterator9 first9 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ ); + } + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Operation > + inline void for_each10( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, + Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , + Iterator9 first9 , Iterator10 first10 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ ); + } + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Operation > + inline void for_each11( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, + Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , + Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ ); + } + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Operation > + inline void for_each12( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, + Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , + Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ ); + } + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Iterator13 , class Operation > + inline void for_each13( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, + Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , + Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Iterator13 first13 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ , *first13++ ); + } + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Iterator13 , class Iterator14 , class Operation > + inline void for_each14( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, + Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , + Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Iterator13 first13 , + Iterator14 first14 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ , *first13++ , *first14++ ); + } + + template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Iterator13 , class Iterator14 , class Iterator15 , class Operation > + inline void for_each15( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, + Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , + Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Iterator13 first13 , + Iterator14 first14 , Iterator15 first15 , Operation op ) + { + for( ; first1 != last1 ; ) + op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ , *first13++ , *first14++ , *first15++ ); + } + + +} // detail +} // odeint +} // numeric +} // boost + + +#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_FOR_EACH_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/macros.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/macros.hpp new file mode 100755 index 0000000..ef97327 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/macros.hpp @@ -0,0 +1,43 @@ +/* + [auto_generated] + boost/numeric/odeint/algebra/detail/macros.hpp + + [begin_description] + Some macros for type checking. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_MACROS_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_MACROS_HPP_INCLUDED + + +//type traits aren't working with nvcc +#ifndef __CUDACC__ +#include +#include + +#define BOOST_ODEINT_CHECK_CONTAINER_TYPE( Type1 , Type2 ) \ + BOOST_STATIC_ASSERT(( boost::is_same< typename boost::remove_const< Type1 >::type , Type2 >::value )) + +#else +//empty macro for nvcc +#define BOOST_ODEINT_CHECK_CONTAINER_TYPE( Type1 , Type2 ) + +#endif // __CUDACC__ + + + +/* +#define BOOST_ODEINT_CHECK_OPERATION_ARITY( Operation , Arity ) \ + BOOST_STATIC_ASSERT(( boost::function_traits< Operation >::arity == Arity )) + */ + +#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_MACROS_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/reduce.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/reduce.hpp new file mode 100755 index 0000000..247d03d --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/reduce.hpp @@ -0,0 +1,67 @@ +/* + [auto_generated] + boost/numeric/odeint/algebra/detail/reduce.hpp + + [begin_description] + Default reduce implementation. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_REDUCE_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_REDUCE_HPP_INCLUDED + + +namespace boost { +namespace numeric { +namespace odeint { +namespace detail { + +template< class ValueType , class Iterator1 , class Reduction > +inline ValueType reduce( Iterator1 first1 , Iterator1 last1 , Reduction red, ValueType init) +{ + for( ; first1 != last1 ; ) + init = red( init , *first1++ ); + return init; +} + + +template< class ValueType , class Iterator1 , class Iterator2 , class Reduction > +inline ValueType reduce2( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Reduction red, ValueType init) +{ + for( ; first1 != last1 ; ) + init = red( init , *first1++ , *first2++ ); + return init; +} + +template< class ValueType , class Iterator1 , class Iterator2 , class Iterator3 , class Reduction > +inline ValueType reduce3( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3 , Reduction red, ValueType init) +{ + for( ; first1 != last1 ; ) + init = red( init , *first1++ , *first2++ , *first3++ ); + return init; +} + +template< class ValueType , class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Reduction > +inline ValueType reduce4( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3 , Iterator4 first4 , Reduction red, ValueType init) +{ + for( ; first1 != last1 ; ) + init = red( init , *first1++ , *first2++ , *first3++ , *first4++ ); + return init; +} + + +} // detail +} // odeint +} // numeric +} // boost + + +#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_REDUCE_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/fusion_algebra.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/fusion_algebra.hpp new file mode 100755 index 0000000..81c4070 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/fusion_algebra.hpp @@ -0,0 +1,186 @@ +/* + [auto_generated] + boost/numeric/odeint/algebra/fusion_algebra.hpp + + [begin_description] + Algebra for boost::fusion sequences. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_FUSION_ALGEBRA_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_ALGEBRA_FUSION_ALGEBRA_HPP_INCLUDED + + +#include +#include +#include +#include +#include + +namespace boost { +namespace numeric { +namespace odeint { + + +struct fusion_algebra +{ + template< class S1 , class Op > + static void for_each1( S1 &s1 , Op op ) + { + boost::fusion::for_each( s1 , op ); + }; + + + template< class S1 , class S2 , class Op > + static void for_each2( S1 &s1 , S2 &s2 , Op op ) + { + typedef boost::fusion::vector< S1& , S2& > Sequences; + Sequences sequences( s1 , s2 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + + template< class S1 , class S2 , class S3 , class Op > + static void for_each3( S1 &s1 , S2 &s2 , S3 &s3 , Op op ) + { + typedef boost::fusion::vector< S1& , S2& , S3& > Sequences; + Sequences sequences( s1 , s2 , s3 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + template< class S1 , class S2 , class S3 , class S4 , class Op > + static void for_each4( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , Op op ) + { + typedef boost::fusion::vector< S1& , S2& , S3& , S4& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class Op > + static void for_each5( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , Op op ) + { + typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 , s5 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class Op > + static void for_each6( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , Op op ) + { + typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class Op > + static void for_each7( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , Op op ) + { + typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class Op > + static void for_each8( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , Op op ) + { + BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 8 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); + typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class Op > + static void for_each9( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , Op op ) + { + BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 9 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); + typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class Op > + static void for_each10( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , Op op ) + { + BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 10 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); + typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class Op > + static void for_each11( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , Op op ) + { + BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 11 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); + BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 11 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" ); + typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class Op > + static void for_each12( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , Op op ) + { + BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 12 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); + BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 12 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" ); + typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class Op > + static void for_each13( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , Op op ) + { + BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 13 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); + BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 13 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" ); + typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& , S13& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class Op > + static void for_each14( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , Op op ) + { + BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 14 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); + BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 14 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" ); + typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& , S13& , S14& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class S15 , class Op > + static void for_each15( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , S15 &s15 , Op op ) + { + BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 15 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" ); + BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 15 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" ); + typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& , S13& , S14& , S15& > Sequences; + Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 , s15 ); + boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) ); + } + + template< class Value , class S , class Reduction > + static Value reduce( const S &s , Reduction red , Value init) + { + return boost::fusion::accumulate( s , init , red ); + } +}; + + + +} // odeint +} // numeric +} // boost + + +#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_FUSION_ALGEBRA_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/range_algebra.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/range_algebra.hpp new file mode 100755 index 0000000..c54e09b --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/range_algebra.hpp @@ -0,0 +1,159 @@ +/* + [auto_generated] + boost/numeric/odeint/algebra/range_algebra.hpp + + [begin_description] + Default algebra, which works with the most state types, like vector< double >, boost::array< double >, boost::range. + Internally is uses boost::range to obtain the begin and end iterator of the according sequence. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_RANGE_ALGEBRA_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_ALGEBRA_RANGE_ALGEBRA_HPP_INCLUDED + +#include +#include + +#include +#include +#include + +namespace boost { +namespace numeric { +namespace odeint { + +struct range_algebra +{ + template< class S1 , class Op > + static void for_each1( S1 &s1 , Op op ) + { + detail::for_each1( boost::begin( s1 ) , boost::end( s1 ) , + op ); + } + + template< class S1 , class S2 , class Op > + static void for_each2( S1 &s1 , S2 &s2 , Op op ) + { + detail::for_each2( boost::begin( s1 ) , boost::end( s1 ) , + boost::begin( s2 ) , op ); + } + + template< class S1 , class S2 , class S3 , class Op > + static void for_each3( S1 &s1 , S2 &s2 , S3 &s3 , Op op ) + { + detail::for_each3( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class Op > + static void for_each4( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , Op op ) + { + detail::for_each4( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class Op > + static void for_each5( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , Op op ) + { + detail::for_each5( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class Op > + static void for_each6( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , Op op ) + { + detail::for_each6( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class Op > + static void for_each7( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , Op op ) + { + detail::for_each7( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class Op > + static void for_each8( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , Op op ) + { + detail::for_each8( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class Op > + static void for_each9( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , Op op ) + { + detail::for_each9( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class Op > + static void for_each10( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , Op op ) + { + detail::for_each10( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class Op > + static void for_each11( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , Op op ) + { + detail::for_each11( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class Op > + static void for_each12( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , Op op ) + { + detail::for_each12( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class Op > + static void for_each13( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , Op op ) + { + detail::for_each13( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , boost::begin( s13 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class Op > + static void for_each14( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , Op op ) + { + detail::for_each14( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , boost::begin( s13 ) , boost::begin( s14 ) , op ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class S15 , class Op > + static void for_each15( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , S15 &s15 , Op op ) + { + detail::for_each15( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , boost::begin( s13 ) , boost::begin( s14 ) , boost::begin( s15 ) , op ); + } + + template< class Value , class S , class Red > + static Value reduce( const S &s , Red red , Value init) + { + return detail::reduce( boost::begin( s ) , boost::end( s ) , red , init ); + } + + template< class Value , class S1 , class S2 , class Red > + static Value reduce2( const S1 &s1 , const S2 &s2 , Red red , Value init ) + { + return detail::reduce2( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , red , init ); + } + + template< class Value , class S1 , class S2 , class S3 , class Red > + static Value reduce3( const S1 &s1 , const S2 &s2 , const S3 &s3 , Red red , Value init ) + { + return detail::reduce3( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , red , init ); + } + + template< class Value , class S1 , class S2 , class S3 , class S4 , class Red > + static Value reduce4( const S1 &s1 , const S2 &s2 , const S3 &s3 , const S4 &s4 , Red red , Value init ) + { + return detail::reduce4( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , red , init ); + } + + +}; + +} // odeint +} // numeric +} // boost + + +#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_RANGE_ALGEBRA_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/vector_space_algebra.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/vector_space_algebra.hpp new file mode 100755 index 0000000..4673b9b --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/vector_space_algebra.hpp @@ -0,0 +1,156 @@ +/* + [auto_generated] + boost/numeric/odeint/algebra/vector_space_algebra.hpp + + [begin_description] + An algebra for types which have vector space semantics, hence types on which the operators +,-,* are well defined. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_VECTOR_SPACE_ALGEBRA_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_ALGEBRA_VECTOR_SPACE_ALGEBRA_HPP_INCLUDED + +#include + + +namespace boost { +namespace numeric { +namespace odeint { + + +/* + * This class template has to be overload in order to call vector_space_algebra::reduce + */ +template< class State > struct vector_space_reduce; + +/* + * Example: instantiation for sole doubles + */ +template<> +struct vector_space_reduce< double > +{ + template< class Op > + double operator()( double x , Op op , double init ) const + { + init = op( init , x ); + return init; + } +}; + + +struct vector_space_algebra +{ + template< class S1 , class Op > + static void for_each1( S1 &s1 , Op op ) + { + // ToDo : build checks, that the +-*/ operators are well defined + op( s1 ); + } + + template< class S1 , class S2 , class Op > + static void for_each2( S1 &s1 , S2 &s2 , Op op ) + { + op( s1 , s2 ); + } + + template< class S1 , class S2 , class S3 , class Op > + static void for_each3( S1 &s1 , S2 &s2 , S3 &s3 , Op op ) + { + op( s1 , s2 , s3 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class Op > + static void for_each4( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , Op op ) + { + op( s1 , s2 , s3 , s4 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class Op > + static void for_each5( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , Op op ) + { + op( s1 , s2 , s3 , s4 , s5 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class Op > + static void for_each6( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , Op op ) + { + op( s1 , s2 , s3 , s4 , s5 , s6 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class Op > + static void for_each7( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , Op op ) + { + op( s1 , s2 , s3 , s4 , s5 , s6 , s7 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class Op > + static void for_each8( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , Op op ) + { + op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class Op > + static void for_each9( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , Op op ) + { + op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class Op > + static void for_each10( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , Op op ) + { + op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class Op > + static void for_each11( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , Op op ) + { + op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class Op > + static void for_each12( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , Op op ) + { + op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class Op > + static void for_each13( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , Op op ) + { + op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class Op > + static void for_each14( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , Op op ) + { + op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 ); + } + + template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class S15 , class Op > + static void for_each15( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , S15 &s15 , Op op ) + { + op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 , s15 ); + } + + template< class Value , class S , class Red > + static Value reduce( const S &s , Red red , Value init ) + { + boost::numeric::odeint::vector_space_reduce< S > r; + return r( s , red , init ); + } +}; + + +} // odeint +} // numeric +} // boost + + +#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_VECTOR_SPACE_ALGEBRA_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/config.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/config.hpp new file mode 100755 index 0000000..0d08982 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/config.hpp @@ -0,0 +1,47 @@ +/* + [auto_generated] + boost/numeric/odeint/config.hpp + + [begin_description] + Sets configurations for odeint and used libraries. Should be included before any other odeint library + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + +#ifndef BOOST_NUMERIC_ODEINT_CONFIG_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_CONFIG_HPP_INCLUDED + +//increase macro variable to allow rk78 scheme +#ifndef FUSION_MAX_VECTOR_SIZE +#define FUSION_MAX_VECTOR_SIZE 15 +#endif + +/* + * the following definitions are only required if fusion vectors are used as state types + * in the rk78 scheme + * they should be defined by the user if required, see e.g. libs/numeric/examples/harmonic_oscillator_units.cpp + */ +#ifndef BOOST_FUSION_INVOKE_MAX_ARITY +#define BOOST_FUSION_INVOKE_MAX_ARITY 15 +#endif + +#ifndef BOOST_RESULT_OF_NUM_ARGS +#define BOOST_RESULT_OF_NUM_ARGS 15 +#endif +/* + */ + +#include + +#if __cplusplus >= 201103L +#define BOOST_NUMERIC_ODEINT_CXX11 1 +#endif + + +#endif // BOOST_NUMERIC_ODEINT_CONFIG_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/gsl/gsl_wrapper.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/gsl/gsl_wrapper.hpp new file mode 100755 index 0000000..c961a89 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/gsl/gsl_wrapper.hpp @@ -0,0 +1,229 @@ +/* + [auto_generated] + boost/numeric/odeint/external/gsl/gsl_wrapper.hpp + + [begin_description] + Wrapper for gsl_vector. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_GSL_GSL_WRAPPER_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_EXTERNAL_GSL_GSL_WRAPPER_HPP_INCLUDED + +#include + +#include + +#include +#include +#include + + +#include +#include +#include + +class const_gsl_vector_iterator; + +/* + * defines an iterator for gsl_vector + */ +class gsl_vector_iterator : public boost::iterator_facade< gsl_vector_iterator , double , boost::random_access_traversal_tag > +{ +public : + + gsl_vector_iterator( void ): m_p(0) , m_stride( 0 ) { } + explicit gsl_vector_iterator( gsl_vector *p ) : m_p( p->data ) , m_stride( p->stride ) { } + friend gsl_vector_iterator end_iterator( gsl_vector * ); + +private : + + friend class boost::iterator_core_access; + friend class const_gsl_vector_iterator; + + void increment( void ) { m_p += m_stride; } + void decrement( void ) { m_p -= m_stride; } + void advance( ptrdiff_t n ) { m_p += n*m_stride; } + bool equal( const gsl_vector_iterator &other ) const { return this->m_p == other.m_p; } + bool equal( const const_gsl_vector_iterator &other ) const; + double& dereference( void ) const { return *m_p; } + + double *m_p; + size_t m_stride; +}; + + + +/* + * defines an const iterator for gsl_vector + */ +class const_gsl_vector_iterator : public boost::iterator_facade< const_gsl_vector_iterator , const double , boost::random_access_traversal_tag > +{ +public : + + const_gsl_vector_iterator( void ): m_p(0) , m_stride( 0 ) { } + explicit const_gsl_vector_iterator( const gsl_vector *p ) : m_p( p->data ) , m_stride( p->stride ) { } + const_gsl_vector_iterator( const gsl_vector_iterator &p ) : m_p( p.m_p ) , m_stride( p.m_stride ) { } + +private : + + friend class boost::iterator_core_access; + friend class gsl_vector_iterator; + friend const_gsl_vector_iterator end_iterator( const gsl_vector * ); + + void increment( void ) { m_p += m_stride; } + void decrement( void ) { m_p -= m_stride; } + void advance( ptrdiff_t n ) { m_p += n*m_stride; } + bool equal( const const_gsl_vector_iterator &other ) const { return this->m_p == other.m_p; } + bool equal( const gsl_vector_iterator &other ) const { return this->m_p == other.m_p; } + const double& dereference( void ) const { return *m_p; } + + const double *m_p; + size_t m_stride; +}; + + +bool gsl_vector_iterator::equal( const const_gsl_vector_iterator &other ) const { return this->m_p == other.m_p; } + + +gsl_vector_iterator end_iterator( gsl_vector *x ) +{ + gsl_vector_iterator iter( x ); + iter.m_p += iter.m_stride * x->size; + return iter; +} + +const_gsl_vector_iterator end_iterator( const gsl_vector *x ) +{ + const_gsl_vector_iterator iter( x ); + iter.m_p += iter.m_stride * x->size; + return iter; +} + + + + +namespace boost +{ +template<> +struct range_mutable_iterator< gsl_vector* > +{ + typedef gsl_vector_iterator type; +}; + +template<> +struct range_const_iterator< gsl_vector* > +{ + typedef const_gsl_vector_iterator type; +}; +} // namespace boost + + + + +// template<> +inline gsl_vector_iterator range_begin( gsl_vector *x ) +{ + return gsl_vector_iterator( x ); +} + +// template<> +inline const_gsl_vector_iterator range_begin( const gsl_vector *x ) +{ + return const_gsl_vector_iterator( x ); +} + +// template<> +inline gsl_vector_iterator range_end( gsl_vector *x ) +{ + return end_iterator( x ); +} + +// template<> +inline const_gsl_vector_iterator range_end( const gsl_vector *x ) +{ + return end_iterator( x ); +} + + + + + + + +namespace boost { +namespace numeric { +namespace odeint { + + +template<> +struct is_resizeable< gsl_vector* > +{ + //struct type : public boost::true_type { }; + typedef boost::true_type type; + const static bool value = type::value; +}; + +template <> +struct same_size_impl< gsl_vector* , gsl_vector* > +{ + static bool same_size( const gsl_vector* x , const gsl_vector* y ) + { + return x->size == y->size; + } +}; + +template <> +struct resize_impl< gsl_vector* , gsl_vector* > +{ + static void resize( gsl_vector* x , const gsl_vector* y ) + { + gsl_vector_free( x ); + x = gsl_vector_alloc( y->size ); + } +}; + +template<> +struct state_wrapper< gsl_vector* > +{ + typedef double value_type; + typedef gsl_vector* state_type; + typedef state_wrapper< gsl_vector* > state_wrapper_type; + + state_type m_v; + + state_wrapper( ) + { + m_v = gsl_vector_alloc( 1 ); + } + + state_wrapper( const state_wrapper_type &x ) + { + resize( m_v , x.m_v ); + gsl_vector_memcpy( m_v , x.m_v ); + } + + + ~state_wrapper() + { + gsl_vector_free( m_v ); + } + +}; + +} // odeint +} // numeric +} // boost + + + + +#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_GSL_GSL_WRAPPER_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mkl/mkl_operations.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mkl/mkl_operations.hpp new file mode 100755 index 0000000..c1e0891 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mkl/mkl_operations.hpp @@ -0,0 +1,181 @@ +/* + [auto_generated] + boost/numeric/odeint/external/mkl/mkl_operations.hpp + + [begin_description] + Wrapper classes for intel math kernel library types. + Get a free, non-commercial download of MKL at + http://software.intel.com/en-us/articles/non-commercial-software-download/ + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED + +#include + +#include +#include + +/* exemplary example for writing bindings to the Intel MKL library + * see test/mkl for how to use mkl with odeint + * this is a quick and dirty implementation showing the general possibility. + * It works only with containers based on double and sequential memory allocation. + */ + +namespace boost { +namespace numeric { +namespace odeint { + +/* only defined for doubles */ +struct mkl_operations +{ + //template< class Fac1 , class Fac2 > struct scale_sum2; + + template< class F1 = double , class F2 = F1 > + struct scale_sum2 + { + typedef double Fac1; + typedef double Fac2; + const Fac1 m_alpha1; + const Fac2 m_alpha2; + + scale_sum2( const Fac1 alpha1 , const Fac2 alpha2 ) : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { } + + template< class T1 , class T2 , class T3 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3) const + { // t1 = m_alpha1 * t2 + m_alpha2 * t3; + // we get Containers that have size() and [i]-access + + const int n = t1.size(); + //boost::numeric::odeint::copy( t1 , t3 ); + if( &(t2[0]) != &(t1[0]) ) + { + cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 ); + } + cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 ); + cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 ); + //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one ); + } + }; + + template< class F1 = double , class F2 = F1 , class F3 = F2 > + struct scale_sum3 + { + typedef double Fac1; + typedef double Fac2; + typedef double Fac3; + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + + scale_sum3( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) { } + + template< class T1 , class T2 , class T3 , class T4 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 ) const + { // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4; + // we get Containers that have size() and [i]-access + + const int n = t1.size(); + //boost::numeric::odeint::copy( t1 , t3 ); + if( &(t2[0]) != &(t1[0]) ) + { + cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 ); + } + cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 ); + cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 ); + //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one ); + cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 ); + } + }; + + template< class F1 = double , class F2 = F1 , class F3 = F2 , class F4 = F3 > + struct scale_sum4 + { + typedef double Fac1; + typedef double Fac2; + typedef double Fac3; + typedef double Fac4; + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + + scale_sum4( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , const Fac4 alpha4 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 ) const + { // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5; + // we get Containers that have size() and [i]-access + + const int n = t1.size(); + //boost::numeric::odeint::copy( t1 , t3 ); + if( &(t2[0]) != &(t1[0]) ) + { + cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 ); + } + + cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 ); + cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 ); + //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one ); + cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 ); + cblas_daxpy( n , m_alpha4 , &(t5[0]) , 1 , &(t1[0]) , 1 ); + } + }; + + + template< class F1 = double , class F2 = F1 , class F3 = F2 , class F4 = F3 , class F5 = F4 > + struct scale_sum5 + { + typedef double Fac1; + typedef double Fac2; + typedef double Fac3; + typedef double Fac4; + typedef double Fac5; + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + + scale_sum5( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , const Fac4 alpha4 , const Fac5 alpha5 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 > + void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 ) const + { // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6; + // we get Containers that have size() and [i]-access + + const int n = t1.size(); + //boost::numeric::odeint::copy( t1 , t3 ); + if( &(t2[0]) != &(t1[0]) ) + { + cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 ); + } + + cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 ); + cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 ); + //daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one ); + cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 ); + cblas_daxpy( n , m_alpha4 , &(t5[0]) , 1 , &(t1[0]) , 1 ); + cblas_daxpy( n , m_alpha5 , &(t6[0]) , 1 , &(t1[0]) , 1 ); + } + }; + +}; + +} // odeint +} // numeric +} // boost + +#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp new file mode 100755 index 0000000..1e7afe6 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp @@ -0,0 +1,162 @@ +/* +[begin_description] +Modification of the implicit Euler method, works with the MTL4 matrix library only. +[end_description] + +Copyright 2009-2011 Karsten Ahnert +Copyright 2009-2011 Mario Mulansky +Copyright 2012 Andreas Angelopoulos + +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or +copy at http://www.boost.org/LICENSE_1_0.txt) +*/ + + +#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED + + +#include + +#include +#include +#include + +#include + +#include +#include + + + + +namespace boost { +namespace numeric { +namespace odeint { + + +template< class ValueType , class Resizer = initially_resizer > +class implicit_euler_mtl4 +{ + +public: + + typedef ValueType value_type; + typedef value_type time_type; + typedef mtl::dense_vector state_type; + + typedef state_wrapper< state_type > wrapped_state_type; + typedef state_type deriv_type; + typedef state_wrapper< deriv_type > wrapped_deriv_type; + typedef mtl::compressed2D< value_type > matrix_type; + typedef state_wrapper< matrix_type > wrapped_matrix_type; + + typedef Resizer resizer_type; + typedef stepper_tag stepper_category; + + typedef implicit_euler_mtl4< ValueType , Resizer > stepper_type; + + + implicit_euler_mtl4( const value_type epsilon = 1E-6 ) + : m_epsilon( epsilon ) , m_resizer() , + m_dxdt() , m_x() , + m_identity() , m_jacobi() + { } + + + template< class System > + void do_step( System system , state_type &x , time_type t , time_type dt ) + { + typedef typename odeint::unwrap_reference< System >::type system_type; + typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type; + typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type; + system_type &sys = system; + deriv_func_type &deriv_func = sys.first; + jacobi_func_type &jacobi_func = sys.second; + + m_resizer.adjust_size( x , detail::bind( + &stepper_type::template resize_impl< state_type > , detail::ref( *this ) , detail::_1 ) ); + + m_identity.m_v = 1; + + t += dt; + m_x.m_v = x; + + deriv_func( x , m_dxdt.m_v , t ); + jacobi_func( x , m_jacobi.m_v , t ); + + + m_dxdt.m_v *= -dt; + + m_jacobi.m_v *= dt; + m_jacobi.m_v -= m_identity.m_v ; + + + + // using ilu_0 preconditioning -incomplete LU factorisation + // itl::pc::diagonal L(m_jacobi.m_v); + itl::pc::ilu_0 L( m_jacobi.m_v ); + + solve( m_jacobi.m_v , m_x.m_v , m_dxdt.m_v , L ); + x+= m_x.m_v; + + + } + + + template< class StateType > + void adjust_size( const StateType &x ) + { + resize_impl( x ); + } + + +private: + + + /* + Applying approximate iterative linear solvers + default solver is Biconjugate gradient stabilized method + itl::bicgstab(A, x, b, L, iter); + */ + template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB, class Preconditioner> + void solve(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b, + const Preconditioner& L, int max_iteractions =500) + { + // Termination criterion: r < 1e-6 * b or N iterations + itl::basic_iteration< double > iter( b , max_iteractions , 1e-6 ); + itl::bicgstab( A , x , b , L , iter ); + + } + + + template< class StateIn > + bool resize_impl( const StateIn &x ) + { + bool resized = false; + resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable::type() ); + resized |= adjust_size_by_resizeability( m_x , x , typename is_resizeable::type() ); + resized |= adjust_size_by_resizeability( m_identity , x , typename is_resizeable::type() ); + resized |= adjust_size_by_resizeability( m_jacobi , x , typename is_resizeable::type() ); + return resized; + } + + +private: + + value_type m_epsilon; + resizer_type m_resizer; + wrapped_deriv_type m_dxdt; + wrapped_state_type m_x; + wrapped_matrix_type m_identity; + wrapped_matrix_type m_jacobi; +}; + + +} // odeint +} // numeric +} // boost + + +#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/mtl4_resize.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/mtl4_resize.hpp new file mode 100755 index 0000000..b7f94d1 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/mtl4_resize.hpp @@ -0,0 +1,133 @@ +/* +[begin_description] +Modification of the implicit Euler method, works with the MTL4 matrix library only. +[end_description] + +Copyright 2009-2011 Karsten Ahnert +Copyright 2009-2011 Mario Mulansky +Copyright 2012 Andreas Angelopoulos + +Distributed under the Boost Software License, Version 1.0. +(See accompanying file LICENSE_1_0.txt or +copy at http://www.boost.org/LICENSE_1_0.txt) +*/ + + +#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED + +#include +#include +#include + +#include +#include +#include + + +namespace boost { +namespace numeric { +namespace odeint { + + +template< class Value , class Parameters > +struct is_resizeable< mtl::dense_vector< Value , Parameters > > +{ + typedef boost::true_type type; + const static bool value = type::value; +}; + +template< class Value , class Parameters > +struct is_resizeable< mtl::dense2D< Value , Parameters > > +{ + typedef boost::true_type type; + const static bool value = type::value; +}; + +template< class Value , class Parameters > +struct is_resizeable< mtl::compressed2D< Value , Parameters > > +{ + typedef boost::true_type type; + const static bool value = type::value; +}; + + + + +template< class Value , class Parameters > +struct same_size_impl< mtl::dense_vector< Value , Parameters > , mtl::dense_vector< Value , Parameters > > +{ + static bool same_size( const mtl::dense_vector< Value , Parameters > &v1 , + const mtl::dense_vector< Value , Parameters > &v2 ) + { + return mtl::size( v1 ) == mtl::size( v2 ); + } +}; + +template< class Value , class Parameters > +struct resize_impl< mtl::dense_vector< Value , Parameters > , mtl::dense_vector< Value , Parameters > > +{ + static void resize( mtl::dense_vector< Value , Parameters > &v1 , + const mtl::dense_vector< Value , Parameters > &v2 ) + { + v1.change_dim( mtl::size( v2 ) ); + } +}; + + + +template< class Value , class MatrixParameters , class VectorParameters > +struct same_size_impl< mtl::dense2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > > +{ + static bool same_size( const mtl::dense2D< Value , MatrixParameters > &m , + const mtl::dense_vector< Value , VectorParameters > &v ) + { + return ( ( mtl::size( v ) == m.num_cols() ) && ( mtl::size( v ) == m.num_rows() ) ); + } +}; + +template< class Value , class MatrixParameters , class VectorParameters > +struct resize_impl< mtl::dense2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > > +{ + static void resize( mtl::dense2D< Value , MatrixParameters > &m , + const mtl::dense_vector< Value , VectorParameters > &v ) + { + m.change_dim( mtl::size( v ) , mtl::size( v ) , false ); + } +}; + + + + +template< class Value , class MatrixParameters , class VectorParameters > +struct same_size_impl< mtl::compressed2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > > +{ + static bool same_size( const mtl::compressed2D< Value , MatrixParameters > &m , + const mtl::dense_vector< Value , VectorParameters > &v ) + { + return ( ( mtl::size( v ) == m.num_cols() ) && ( mtl::size( v ) == m.num_rows() ) ); + } +}; + +template< class Value , class MatrixParameters , class VectorParameters > +struct resize_impl< mtl::compressed2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > > +{ + static void resize( mtl::compressed2D< Value , MatrixParameters > &m , + const mtl::dense_vector< Value , VectorParameters > &v ) + { + m.change_dim( mtl::size( v ) , mtl::size( v ) ); + } +}; + + + + + + + + +} // namespace odeint +} // namespace numeric +} // namespace boost + +#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_algebra.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_algebra.hpp new file mode 100755 index 0000000..0f6b593 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_algebra.hpp @@ -0,0 +1,198 @@ +/* + [auto_generated] + boost/numeric/odeint/external/thrust/thrust_algebra.hpp + + [begin_description] + An algebra for thrusts device_vectors. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_ALGEBRA_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_ALGEBRA_HPP_INCLUDED + + +#include +#include +#include + +#include + +namespace boost { +namespace numeric { +namespace odeint { + + + +/** ToDO extend until for_each14 for rk78 */ + +/* + * The const versions are needed for boost.range to work, i.e. + * it allows you to do + * for_each1( make_pair( vec1.begin() , vec1.begin() + 10 ) , op ); + */ + +struct thrust_algebra +{ + template< class StateType , class Operation > + static void for_each1( StateType &s , Operation op ) + { + thrust::for_each( boost::begin(s) , boost::begin(s) , op ); + } + + template< class StateType1 , class StateType2 , class Operation > + static void for_each2( StateType1 &s1 , StateType2 &s2 , Operation op ) + { + thrust::for_each( + thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , + boost::begin(s2) ) ) , + thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , + boost::end(s2) ) ) , + op); + } + + template< class StateType1 , class StateType2 , class StateType3 , class Operation > + static void for_each3( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , Operation op ) + { + thrust::for_each( + thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , + boost::begin(s2) , + boost::begin(s3) ) ) , + thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , + boost::end(s2) , + boost::end(s3) ) ) , + op); + } + + template< class StateType1 , class StateType2 , class StateType3 , class StateType4 , + class Operation > + static void for_each4( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 , + Operation op ) + { + thrust::for_each( + thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , + boost::begin(s2) , + boost::begin(s3) , + boost::begin(s4) ) ) , + thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , + boost::end(s2) , + boost::end(s3) , + boost::end(s4) ) ) , + op); + } + + template< class StateType1 , class StateType2 , class StateType3 , + class StateType4 , class StateType5 ,class Operation > + static void for_each5( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 , + StateType5 &s5 , Operation op ) + { + thrust::for_each( + thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , + boost::begin(s2) , + boost::begin(s3) , + boost::begin(s4) , + boost::begin(s5) ) ) , + thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , + boost::end(s2) , + boost::end(s3) , + boost::end(s4) , + boost::end(s5) ) ) , + op); + } + + template< class StateType1 , class StateType2 , class StateType3 , + class StateType4 , class StateType5 , class StateType6 , class Operation > + static void for_each6( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 , + StateType5 &s5 , StateType6 &s6 , Operation op ) + { + thrust::for_each( + thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , + boost::begin(s2) , + boost::begin(s3) , + boost::begin(s4) , + boost::begin(s5) , + boost::begin(s6) ) ) , + thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , + boost::end(s2) , + boost::end(s3) , + boost::end(s4) , + boost::end(s5) , + boost::end(s6) ) ) , + op); + } + + template< class StateType1 , class StateType2 , class StateType3 , class StateType4 , + class StateType5 , class StateType6 , class StateType7 , class Operation > + static void for_each7( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 , + StateType5 &s5 , StateType6 &s6 , StateType7 &s7 , Operation op ) + { + thrust::for_each( + thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , + boost::begin(s2) , + boost::begin(s3) , + boost::begin(s4) , + boost::begin(s5) , + boost::begin(s6) , + boost::begin(s7) ) ) , + thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , + boost::end(s2) , + boost::end(s3) , + boost::end(s4) , + boost::end(s5) , + boost::end(s6) , + boost::end(s7) ) ) , + op); + } + + template< class StateType1 , class StateType2 , class StateType3 , class StateType4 , + class StateType5 , class StateType6 , class StateType7 , class StateType8 , class Operation > + static void for_each8( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 , + StateType5 &s5 , StateType6 &s6 , StateType7 &s7 , StateType8 &s8 , Operation op ) + { + thrust::for_each( + thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) , + boost::begin(s2) , + boost::begin(s3) , + boost::begin(s4) , + boost::begin(s5) , + boost::begin(s6) , + boost::begin(s7) , + boost::begin(s8) ) ) , + thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) , + boost::end(s2) , + boost::end(s3) , + boost::end(s4) , + boost::end(s5) , + boost::end(s6) , + boost::end(s7) , + boost::end(s8) ) ) , + op); + } + + + template< class Value , class S , class Red > + Value reduce( const S &s , Red red , Value init) + { + return thrust::reduce( boost::begin( s ) , boost::end( s ) , init , red ); + } + + + + +}; + + +} // odeint +} // numeric +} // boost + + + +#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_ALGEBRA_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_operations.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_operations.hpp new file mode 100755 index 0000000..710657a --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_operations.hpp @@ -0,0 +1,252 @@ +/* + [auto_generated] + boost/numeric/odeint/external/thrust/thrust_operations.hpp + + [begin_description] + Operations of thrust zipped iterators. Is the counterpart of the thrust_algebra. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_OPERATIONS_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_OPERATIONS_HPP_INCLUDED + +namespace boost { +namespace numeric { +namespace odeint { + +#include +#include + +/**ToDo extend to scale_sum13 for rk78 */ + +struct thrust_operations +{ + template< class Fac1 = double , class Fac2 = Fac1 > + struct scale_sum2 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + + scale_sum2( const Fac1 alpha1 , const Fac2 alpha2 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { } + + template< class Tuple > + __host__ __device__ + void operator()( Tuple t ) const + { + thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + m_alpha2 * thrust::get<2>(t); + } + }; + + template< class Fac1 = double , class Fac2 = Fac1 > + struct scale_sum_swap2 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + + scale_sum_swap2( const Fac1 alpha1 , const Fac2 alpha2 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { } + + template< class Tuple > + __host__ __device__ + void operator()( Tuple t ) const + { + typename thrust::tuple_element<0,Tuple>::type tmp = thrust::get<0>(t); + thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + m_alpha2 * thrust::get<2>(t); + thrust::get<1>(t) = tmp; + } + }; + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 > + struct scale_sum3 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + + scale_sum3( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) { } + + template< class Tuple > + __host__ __device__ + void operator()( Tuple t ) const + { + thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + + m_alpha2 * thrust::get<2>(t) + + m_alpha3 * thrust::get<3>(t); + } + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 > + struct scale_sum4 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + + scale_sum4( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , const Fac4 alpha4 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ){ } + + template< class Tuple > + __host__ __device__ + void operator()( Tuple t ) const + { + thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + + m_alpha2 * thrust::get<2>(t) + + m_alpha3 * thrust::get<3>(t) + + m_alpha4 * thrust::get<4>(t); + } + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , + class Fac4 = Fac3 , class Fac5 = Fac4 > + struct scale_sum5 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + + scale_sum5( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , + const Fac4 alpha4 , const Fac5 alpha5 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , + m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { } + + template< class Tuple > + __host__ __device__ + void operator()( Tuple t ) const + { + thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + + m_alpha2 * thrust::get<2>(t) + + m_alpha3 * thrust::get<3>(t) + + m_alpha4 * thrust::get<4>(t) + + m_alpha5 * thrust::get<5>(t); + } + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , + class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 > + struct scale_sum6 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + + scale_sum6( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , + const Fac4 alpha4 , const Fac5 alpha5 , const Fac6 alpha6 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , + m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) { } + + template< class Tuple > + __host__ __device__ + void operator()( Tuple t ) const + { + thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + + m_alpha2 * thrust::get<2>(t) + + m_alpha3 * thrust::get<3>(t) + + m_alpha4 * thrust::get<4>(t) + + m_alpha5 * thrust::get<5>(t) + + m_alpha6 * thrust::get<6>(t); + } + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , + class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 > + struct scale_sum7 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + + scale_sum7( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , + const Fac4 alpha4 , const Fac5 alpha5 , const Fac6 alpha6 , const Fac7 alpha7 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , + m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) { } + + template< class Tuple > + __host__ __device__ + void operator()( Tuple t ) const + { + thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + + m_alpha2 * thrust::get<2>(t) + + m_alpha3 * thrust::get<3>(t) + + m_alpha4 * thrust::get<4>(t) + + m_alpha5 * thrust::get<5>(t) + + m_alpha6 * thrust::get<6>(t) + + m_alpha7 * thrust::get<7>(t) ; + } + }; + + + + + template< class Fac1 = double > + struct rel_error + { + const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt; + + rel_error( const Fac1 eps_abs , const Fac1 eps_rel , const Fac1 a_x , const Fac1 a_dxdt ) + : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt ) { } + + + template< class Tuple > + __host__ __device__ + void operator()( Tuple t ) const + { + using std::abs; + thrust::get< 0 >( t ) = abs( thrust::get< 0 >( t ) ) / + ( m_eps_abs + m_eps_rel * ( m_a_x * abs( thrust::get< 1 >( t ) + m_a_dxdt * abs( thrust::get< 2 >( t ) ) ) ) ); + } + + typedef void result_type; + }; + + + /* + * for usage in reduce + */ + + template< class Value > + struct maximum + { + template< class Fac1 , class Fac2 > + __host__ __device__ + Value operator()( const Fac1 t1 , const Fac2 t2 ) const + { + using std::max; + return ( abs( t1 ) < abs( t2 ) ) ? t2 : t1 ; + } + + typedef Value result_type; + }; + + +}; + +} // odeint +} // numeric +} // boost + + +#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_OPERATIONS_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_resize.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_resize.hpp new file mode 100755 index 0000000..843a100 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_resize.hpp @@ -0,0 +1,119 @@ +/* + [auto_generated] + boost/numeric/odeint/external/thrust/thrust_resize.hpp + + [begin_description] + Enable resizing for thrusts device and host_vector. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_RESIZE_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_RESIZE_HPP_INCLUDED + + +#include +#include + +#include + +namespace boost { +namespace numeric { +namespace odeint { + +template< class T > +struct is_resizeable< thrust::device_vector< T > > +{ + struct type : public boost::true_type { }; + const static bool value = type::value; +}; + +template< class T > +struct same_size_impl< thrust::device_vector< T > , thrust::device_vector< T > > +{ + static bool same_size( const thrust::device_vector< T > &x , const thrust::device_vector< T > &y ) + { + return x.size() == y.size(); + } +}; + +template< class T > +struct resize_impl< thrust::device_vector< T > , thrust::device_vector< T > > +{ + static void resize( thrust::device_vector< T > &x , const thrust::device_vector< T > &y ) + { + x.resize( y.size() ); + } +}; + + +template< class T > +struct is_resizeable< thrust::host_vector< T > > +{ + struct type : public boost::true_type { }; + const static bool value = type::value; +}; + +template< class T > +struct same_size_impl< thrust::host_vector< T > , thrust::host_vector< T > > +{ + static bool same_size( const thrust::host_vector< T > &x , const thrust::host_vector< T > &y ) + { + return x.size() == y.size(); + } +}; + +template< class T > +struct resize_impl< thrust::host_vector< T > , thrust::host_vector< T > > +{ + static void resize( thrust::host_vector< T > &x , const thrust::host_vector< T > &y ) + { + x.resize( y.size() ); + } +}; + + + +template< class Container1, class Value > +struct copy_impl< Container1 , thrust::device_vector< Value > > +{ + static void copy( const Container1 &from , thrust::device_vector< Value > &to ) + { + thrust::copy( boost::begin( from ) , boost::end( from ) , boost::begin( to ) ); + } +}; + +template< class Value , class Container2 > +struct copy_impl< thrust::device_vector< Value > , Container2 > +{ + static void copy( const thrust::device_vector< Value > &from , Container2 &to ) + { + thrust::copy( boost::begin( from ) , boost::end( from ) , boost::begin( to ) ); + } +}; + +template< class Value > +struct copy_impl< thrust::device_vector< Value > , thrust::device_vector< Value > > +{ + static void copy( const thrust::device_vector< Value > &from , thrust::device_vector< Value > &to ) + { + thrust::copy( boost::begin( from ) , boost::end( from ) , boost::begin( to ) ); + } +}; + + + + +} // odeint +} // numeric +} // boost + + +#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_RESIZE_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/vexcl/vexcl_resize.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/vexcl/vexcl_resize.hpp new file mode 100755 index 0000000..e9cbea3 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/vexcl/vexcl_resize.hpp @@ -0,0 +1,92 @@ +/* + [auto_generated] + boost/numeric/odeint/external/vexcl/vexcl_resize.hpp + + [begin_description] + Enable resizing for vexcl vector and multivector. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_RESIZE_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_RESIZE_HPP_INCLUDED + +#include + +#include +#include +#include + +namespace boost { +namespace numeric { +namespace odeint { + + + +/* + * specializations for vex::vector< T > + */ +template< typename T > +struct is_resizeable< vex::vector< T > > : boost::true_type { }; + +template< typename T > +struct resize_impl< vex::vector< T > , vex::vector< T > > +{ + static void resize( vex::vector< T > &x1 , const vex::vector< T > &x2 ) + { + x1.resize( x2.queue_list() , x2.size() ); + } +}; + +template< typename T > +struct same_size_impl< vex::vector< T > , vex::vector< T > > +{ + static bool same_size( const vex::vector< T > &x1 , const vex::vector< T > &x2 ) + { + return x1.size() == x2.size(); + } +}; + + + + + +/* + * specializations for vex::multivector< T > + */ +template< typename T , size_t N, bool own > +struct is_resizeable< vex::multivector< T , N , own > > : boost::true_type { }; + +template< typename T , size_t N, bool own > +struct resize_impl< vex::multivector< T , N , own > , vex::multivector< T , N , own > > +{ + static void resize( vex::multivector< T , N , own > &x1 , const vex::multivector< T , N , own > &x2 ) + { + x1.resize( x2.queue_list() , x2.size() ); + } +}; + +template< typename T , size_t N, bool own > +struct same_size_impl< vex::multivector< T , N , own > , vex::multivector< T , N , own > > +{ + static bool same_size( const vex::multivector< T , N , own > &x1 , const vex::multivector< T , N , own > &x2 ) + { + return x1.size() == x2.size(); + } +}; + + +} // namespace odeint +} // namespace numeric +} // namespace boost + + + +#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_RESIZE_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_operations.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_operations.hpp new file mode 100755 index 0000000..d621b35 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_operations.hpp @@ -0,0 +1,245 @@ +/* + [auto_generated] + boost/numeric/odeint/external/viennacl_operations.hpp + + [begin_description] + ViennaCL operations. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_OPERATIONS_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_OPERATIONS_HPP_INCLUDED + +#include +#include + +namespace boost { +namespace numeric { +namespace odeint { + + + +struct viennacl_operations +{ + + template< class Fac1 = double , class Fac2 = Fac1 > + struct scale_sum2 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + + scale_sum2( Fac1 alpha1 , Fac2 alpha2 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) + { } + + template< class T1 , class T2 , class T3 > + void operator()( viennacl::vector &v1 , + const viennacl::vector &v2 , + const viennacl::vector &v3 + ) const + { + using namespace viennacl; + + static generator::symbolic_vector <0, T1> sym_v1; + static generator::symbolic_vector <1, T2> sym_v2; + static generator::symbolic_vector <2, T3> sym_v3; + static generator::cpu_symbolic_scalar<3, Fac1> sym_a1; + static generator::cpu_symbolic_scalar<4, Fac2> sym_a2; + + static generator::custom_operation op( + sym_v1 = sym_a1 * sym_v2 + + sym_a2 * sym_v3 + ); + + ocl::enqueue( op(v1, + const_cast< viennacl::vector& >(v2), + const_cast< viennacl::vector& >(v3), + const_cast< Fac1& >(m_alpha1), + const_cast< Fac2& >(m_alpha2) + ) ); + } + + typedef void result_type; + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 > + struct scale_sum3 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + + scale_sum3( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) + { } + + template< class T1 , class T2 , class T3 , class T4 > + void operator()( viennacl::vector &v1 , + const viennacl::vector &v2 , + const viennacl::vector &v3 , + const viennacl::vector &v4 + ) const + { + using namespace viennacl; + + static generator::symbolic_vector <0, T1> sym_v1; + static generator::symbolic_vector <1, T2> sym_v2; + static generator::symbolic_vector <2, T3> sym_v3; + static generator::symbolic_vector <3, T4> sym_v4; + static generator::cpu_symbolic_scalar<4, Fac1> sym_a1; + static generator::cpu_symbolic_scalar<5, Fac2> sym_a2; + static generator::cpu_symbolic_scalar<6, Fac3> sym_a3; + + static generator::custom_operation op( + sym_v1 = sym_a1 * sym_v2 + + sym_a2 * sym_v3 + + sym_a3 * sym_v4 + ); + + ocl::enqueue( op(v1, + const_cast< viennacl::vector& >(v2), + const_cast< viennacl::vector& >(v3), + const_cast< viennacl::vector& >(v4), + const_cast< Fac1& >(m_alpha1), + const_cast< Fac2& >(m_alpha2), + const_cast< Fac3& >(m_alpha3) + ) ); + } + + typedef void result_type; + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 > + struct scale_sum4 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + + scale_sum4( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 > + void operator()( viennacl::vector &v1 , + const viennacl::vector &v2 , + const viennacl::vector &v3 , + const viennacl::vector &v4 , + const viennacl::vector &v5 + ) const + { + using namespace viennacl; + + static generator::symbolic_vector <0, T1> sym_v1; + static generator::symbolic_vector <1, T2> sym_v2; + static generator::symbolic_vector <2, T3> sym_v3; + static generator::symbolic_vector <3, T4> sym_v4; + static generator::symbolic_vector <4, T5> sym_v5; + static generator::cpu_symbolic_scalar<5, Fac1> sym_a1; + static generator::cpu_symbolic_scalar<6, Fac2> sym_a2; + static generator::cpu_symbolic_scalar<7, Fac3> sym_a3; + static generator::cpu_symbolic_scalar<8, Fac4> sym_a4; + + static generator::custom_operation op( + sym_v1 = sym_a1 * sym_v2 + + sym_a2 * sym_v3 + + sym_a3 * sym_v4 + + sym_a4 * sym_v5 + ); + + ocl::enqueue( op(v1, + const_cast< viennacl::vector& >(v2), + const_cast< viennacl::vector& >(v3), + const_cast< viennacl::vector& >(v4), + const_cast< viennacl::vector& >(v5), + const_cast< Fac1& >(m_alpha1), + const_cast< Fac2& >(m_alpha2), + const_cast< Fac3& >(m_alpha3), + const_cast< Fac4& >(m_alpha4) + ) ); + } + + typedef void result_type; + }; + + + template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 > + struct scale_sum5 + { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + + scale_sum5( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , Fac5 alpha5 ) + : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { } + + template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 > + void operator()( viennacl::vector &v1 , + const viennacl::vector &v2 , + const viennacl::vector &v3 , + const viennacl::vector &v4 , + const viennacl::vector &v5 , + const viennacl::vector &v6 + ) const + { + using namespace viennacl; + + static generator::symbolic_vector < 0, T1> sym_v1; + static generator::symbolic_vector < 1, T2> sym_v2; + static generator::symbolic_vector < 2, T3> sym_v3; + static generator::symbolic_vector < 3, T4> sym_v4; + static generator::symbolic_vector < 4, T5> sym_v5; + static generator::symbolic_vector < 5, T6> sym_v6; + static generator::cpu_symbolic_scalar< 6, Fac1> sym_a1; + static generator::cpu_symbolic_scalar< 7, Fac2> sym_a2; + static generator::cpu_symbolic_scalar< 8, Fac3> sym_a3; + static generator::cpu_symbolic_scalar< 9, Fac4> sym_a4; + static generator::cpu_symbolic_scalar<10, Fac5> sym_a5; + + static generator::custom_operation op( + sym_v1 = sym_a1 * sym_v2 + + sym_a2 * sym_v3 + + sym_a3 * sym_v4 + + sym_a4 * sym_v5 + + sym_a5 * sym_v6 + ); + + ocl::enqueue( op(v1, + const_cast< viennacl::vector& >(v2), + const_cast< viennacl::vector& >(v3), + const_cast< viennacl::vector& >(v4), + const_cast< viennacl::vector& >(v5), + const_cast< viennacl::vector& >(v6), + const_cast< Fac1& >(m_alpha1), + const_cast< Fac2& >(m_alpha2), + const_cast< Fac3& >(m_alpha3), + const_cast< Fac4& >(m_alpha4), + const_cast< Fac5& >(m_alpha5) + ) ); + } + + typedef void result_type; + }; + + +}; + + +} // odeint +} // numeric +} // boost + + +#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_OPERATIONS_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_resize.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_resize.hpp new file mode 100755 index 0000000..677cfce --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_resize.hpp @@ -0,0 +1,65 @@ +/* + [auto_generated] + boost/numeric/odeint/external/viennacl/viennacl_resize.hpp + + [begin_description] + Enable resizing for viennacl vector. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_RESIZE_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_RESIZE_HPP_INCLUDED + +#include + +#include +#include +#include + +namespace boost { +namespace numeric { +namespace odeint { + + + +/* + * specializations for viennacl::vector< T > + */ +template< typename T > +struct is_resizeable< viennacl::vector< T > > : boost::true_type { }; + +template< typename T > +struct resize_impl< viennacl::vector< T > , viennacl::vector< T > > +{ + static void resize( viennacl::vector< T > &x1 , const viennacl::vector< T > &x2 ) + { + x1.resize( x2.size() , false ); + } +}; + +template< typename T > +struct same_size_impl< viennacl::vector< T > , viennacl::vector< T > > +{ + static bool same_size( const viennacl::vector< T > &x1 , const viennacl::vector< T > &x2 ) + { + return x1.size() == x2.size(); + } +}; + + + +} // namespace odeint +} // namespace numeric +} // namespace boost + + + +#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_RESIZE_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp new file mode 100755 index 0000000..071159f --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp @@ -0,0 +1,154 @@ +/* + [auto_generated] + boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp + + [begin_description] + Default Integrate adaptive implementation. + [end_description] + + Copyright 2009-2011 Karsten Ahnert + Copyright 2009-2011 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + + +#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED + +#include + +#include +#include +#include +#include +#include +#include + +#include + + +#include + +namespace boost { +namespace numeric { +namespace odeint { +namespace detail { + +// forward declaration +template< class Stepper , class System , class State , class Time , class Observer> +size_t integrate_const( + Stepper stepper , System system , State &start_state , + Time start_time , Time end_time , Time dt , + Observer observer , stepper_tag ); + +/* + * integrate_adaptive for simple stepper is basically an integrate_const + some last step + */ +template< class Stepper , class System , class State , class Time , class Observer > +size_t integrate_adaptive( + Stepper stepper , System system , State &start_state , + Time start_time , Time end_time , Time dt , + Observer observer , stepper_tag +) +{ + size_t steps = detail::integrate_const( stepper , system , start_state , start_time , + end_time , dt , observer , stepper_tag() ); + Time end = start_time + dt*steps; + if( less_with_sign( end , end_time , dt ) ) + { //make a last step to end exactly at end_time + stepper.do_step( system , start_state , end , end_time - end ); + steps++; + typename odeint::unwrap_reference< Observer >::type &obs = observer; + obs( start_state , end_time ); + } + return steps; +} + + +/* + * classical integrate adaptive + */ +template< class Stepper , class System , class State , class Time , class Observer > +size_t integrate_adaptive( + Stepper stepper , System system , State &start_state , + Time &start_time , Time end_time , Time &dt , + Observer observer , controlled_stepper_tag +) +{ + typename odeint::unwrap_reference< Observer >::type &obs = observer; + + const size_t max_attempts = 1000; + const char *error_string = "Integrate adaptive : Maximal number of iterations reached. A step size could not be found."; + size_t count = 0; + while( less_with_sign( start_time , end_time , dt ) ) + { + obs( start_state , start_time ); + if( less_with_sign( end_time , start_time + dt , dt ) ) + { + dt = end_time - start_time; + } + + size_t trials = 0; + controlled_step_result res = success; + do + { + res = stepper.try_step( system , start_state , start_time , dt ); + ++trials; + } + while( ( res == fail ) && ( trials < max_attempts ) ); + if( trials == max_attempts ) throw std::overflow_error( error_string ); + + ++count; + } + obs( start_state , start_time ); + return count; +} + + +/* + * integrate adaptive for dense output steppers + * + * step size control is used if the stepper supports it + */ +template< class Stepper , class System , class State , class Time , class Observer > +size_t integrate_adaptive( + Stepper stepper , System system , State &start_state , + Time start_time , Time end_time , Time dt , + Observer observer , dense_output_stepper_tag ) +{ + typename odeint::unwrap_reference< Observer >::type &obs = observer; + + size_t count = 0; + stepper.initialize( start_state , start_time , dt ); + + while( less_with_sign( stepper.current_time() , end_time , stepper.current_time_step() ) ) + { + while( less_eq_with_sign( stepper.current_time() + stepper.current_time_step() , + end_time , + stepper.current_time_step() ) ) + { //make sure we don't go beyond the end_time + obs( stepper.current_state() , stepper.current_time() ); + stepper.do_step( system ); + ++count; + } + stepper.initialize( stepper.current_state() , stepper.current_time() , end_time - stepper.current_time() ); + } + obs( stepper.current_state() , stepper.current_time() ); + // overwrite start_state with the final point + boost::numeric::odeint::copy( stepper.current_state() , start_state ); + return count; +} + + + + +} // namespace detail +} // namespace odeint +} // namespace numeric +} // namespace boost + + +#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED diff --git a/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_const.hpp b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_const.hpp new file mode 100755 index 0000000..2eb7ae0 --- /dev/null +++ b/motion_planning/3d/ego_planner/uav_simulator/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_const.hpp @@ -0,0 +1,159 @@ +/* + [auto_generated] + boost/numeric/odeint/integrate/detail/integrate_const.hpp + + [begin_description] + integrate const implementation + [end_description] + + Copyright 2009-2012 Karsten Ahnert + Copyright 2009-2012 Mario Mulansky + + Distributed under the Boost Software License, Version 1.0. + (See accompanying file LICENSE_1_0.txt or + copy at http://www.boost.org/LICENSE_1_0.txt) + */ + +#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_CONST_HPP_INCLUDED +#define BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_CONST_HPP_INCLUDED + +#include +#include +#include +#include + +#include + +namespace boost { +namespace numeric { +namespace odeint { +namespace detail { + +// forward declaration +template< class Stepper , class System , class State , class Time , class Observer > +size_t integrate_adaptive( + Stepper stepper , System system , State &start_state , + Time &start_time , Time end_time , Time &dt , + Observer observer , controlled_stepper_tag +); + + +template< class Stepper , class System , class State , class Time , class Observer > +size_t integrate_const( + Stepper stepper , System system , State &start_state , + Time start_time , Time end_time , Time dt , + Observer observer , stepper_tag +) +{ + typename odeint::unwrap_reference< Observer >::type &obs = observer; + + Time time = start_time; + int step = 0; + + while( less_eq_with_sign( time+dt , end_time , dt ) ) + { + obs( start_state , time ); + stepper.do_step( system , start_state , time , dt ); + // direct computation of the time avoids error propagation happening when using time += dt + // we need clumsy type analysis to get boost units working here + ++step; + time = start_time + static_cast< typename unit_value_type