├── Communication ├── multisim_gazebo │ ├── CMakeLists.txt │ ├── include │ │ └── multisim_gazebo │ │ │ └── multi_rotors.h │ ├── launch │ │ ├── demo_2drones.launch │ │ ├── rotors_hull_sim.launch │ │ └── rotors_sim.launch │ ├── msg │ │ ├── IdOdom.msg │ │ └── states.msg │ ├── package.xml │ ├── resource │ │ └── 2_1_rotors.yaml │ ├── src │ │ ├── multi_rotors.cpp │ │ └── multi_rotors_node.cpp │ └── worlds │ │ ├── amagi1.world │ │ ├── building.world │ │ ├── edgar_mine.world │ │ ├── flat.world │ │ ├── flat3.world │ │ ├── forest.world │ │ ├── large.world │ │ ├── large_maze.world │ │ ├── large_maze_lid.world │ │ ├── maze1.world │ │ ├── maze2.world │ │ ├── maze3.world │ │ ├── maze4.world │ │ ├── power_plant.world │ │ └── tunnel.world ├── swarm_bridge │ ├── .vscode │ │ ├── c_cpp_properties.json │ │ └── settings.json │ ├── CMakeLists.txt │ ├── include │ │ ├── bridge_ros_tcp.h │ │ ├── bridge_ros_tcp_sim.h │ │ ├── bridge_ros_udp.h │ │ └── reliable_bridge.hpp │ ├── launch │ │ ├── local_tcp.launch │ │ └── test_tcp_drone2.launch │ ├── note.md │ ├── package.xml │ └── src │ │ ├── bridge_ros_tcp.cpp │ │ ├── bridge_ros_tcp_sim.cpp │ │ └── bridge_ros_udp.cpp └── swarm_data │ ├── CMakeLists.txt │ ├── include │ └── swarm_data │ │ └── swarm_data.h │ ├── package.xml │ └── src │ └── swarm_data.cpp ├── Exploration ├── graph_partition │ ├── CMakeLists.txt │ ├── include │ │ └── graph_partition │ │ │ └── graph_partition.h │ ├── package.xml │ └── src │ │ └── graph_partition.cpp ├── multiDTG │ ├── CMakeLists.txt │ ├── include │ │ └── multiDTG │ │ │ ├── dtg_structures.h │ │ │ └── multiDTG.h │ ├── package.xml │ └── src │ │ ├── multiDTG.cpp │ │ └── multiDTG_path_search.cpp └── murder_swarm │ ├── CMakeLists.txt │ ├── include │ └── murder_swarm │ │ ├── ground.h │ │ ├── murder.h │ │ └── murderFSM.h │ ├── launch │ ├── debug.launch │ ├── murder_ground_demo.launch │ ├── murder_single.launch │ ├── murder_swarm_maze3.launch │ ├── murder_swarm_maze4.launch │ ├── murder_swarm_testcity.launch │ ├── murder_swarm_trigger.launch │ ├── rotors_sim.launch │ └── uav_model.launch │ ├── package.xml │ ├── resource │ ├── debug.yaml │ ├── murder_maze3.yaml │ ├── murder_maze3_ground.yaml │ ├── murder_maze4.yaml │ ├── murder_maze4_ground.yaml │ ├── murder_swarm2_demo.yaml │ ├── murder_testcity.yaml │ └── murder_testcity_ground.yaml │ ├── rviz │ ├── debug.rviz │ ├── ground_demo.rviz │ ├── murder_demo.rviz │ ├── murder_swarm2_demo.rviz │ └── murder_swarm_local.rviz │ └── src │ ├── ground.cpp │ ├── ground_node.cpp │ ├── murder.cpp │ ├── murderFSM.cpp │ ├── murder_node.cpp │ └── swarm_trigger.cpp ├── LICENSE ├── MSGS ├── exp_comm_msgs │ ├── CMakeLists.txt │ ├── msg │ │ ├── DtgBagAnswer.msg │ │ ├── DtgBagC.msg │ │ ├── IdPoseC.msg │ │ ├── MapC.msg │ │ ├── MapReqC.msg │ │ ├── SwarmJobC.msg │ │ ├── SwarmStateC.msg │ │ └── SwarmTrajC.msg │ └── package.xml └── swarm_exp_msgs │ ├── CMakeLists.txt │ ├── msg │ ├── DtgBag.msg │ ├── DtgFFEdge.msg │ ├── DtgFNode.msg │ ├── DtgHFEdge.msg │ ├── DtgHHEdge.msg │ ├── DtgHNode.msg │ ├── IdOdom.msg │ ├── IdPose.msg │ ├── LocalTraj.msg │ └── SwarmTraj.msg │ └── package.xml ├── Mapping ├── block_map │ ├── CMakeLists.txt │ ├── color │ │ ├── Abbey_Road.png │ │ ├── Abbey_Road.yaml │ │ ├── Abbey_road.jpg │ │ ├── Burn.jpg │ │ ├── Burn.png │ │ ├── Burn.yaml │ │ ├── DrFeelgood.jpg │ │ ├── DrFeelgood.png │ │ ├── DrFeelgood.yaml │ │ ├── Odessey_Oracle.jpg │ │ ├── Odessey_Oracle.png │ │ ├── Odessey_Oracle.yaml │ │ ├── Paranoid.png │ │ ├── Paranoid.yaml │ │ ├── Powerslave.png │ │ ├── Powerslave.yaml │ │ ├── TDSOTM.jpg │ │ ├── TDSOTM.png │ │ ├── TDSOTM.yaml │ │ ├── ViewpointColor.yaml │ │ ├── color_id.yaml │ │ ├── paranoid.jpg │ │ └── powerslave.jpg │ ├── include │ │ └── block_map │ │ │ ├── block_map.h │ │ │ ├── color_manager.h │ │ │ ├── mapping_struct.h │ │ │ └── raycast.h │ ├── package.xml │ ├── rviz │ │ └── showvox.rviz │ └── src │ │ ├── block_map.cpp │ │ ├── color_manager.cpp │ │ └── raycast.cpp ├── frontier_grid │ ├── CMakeLists.txt │ ├── include │ │ └── frontier_grid │ │ │ ├── frontier_grid.h │ │ │ └── frontier_struct.h │ ├── package.xml │ ├── rviz │ │ └── showvox.rviz │ └── src │ │ └── frontier_grid.cpp └── lowres_map │ ├── CMakeLists.txt │ ├── Readme.md │ ├── include │ └── lowres_map │ │ └── lowres_map.h │ ├── package.xml │ └── src │ ├── lowres_map.cpp │ └── path_search.cpp ├── README.md ├── Test └── ato_test │ ├── CMakeLists.txt │ ├── launch │ ├── cmd_test.launch │ ├── fov_ray.launch │ ├── swarm_sim │ │ └── swarm_vis.launch │ └── swarm_traj.launch │ ├── package.xml │ ├── resource │ ├── example.yaml │ ├── fov_cam.yaml │ ├── multi_robots_gazebo.yaml │ └── multirobots.yaml │ ├── rviz │ ├── DTG.rviz │ ├── MR_DTG.rviz │ ├── blockmap_lowres.rviz │ ├── cmd_test.rviz │ ├── corridor.rviz │ ├── fov_gain.rviz │ ├── frontier.rviz │ ├── minco.rviz │ ├── swarm_test.rviz │ └── swarm_traj.rviz │ ├── src │ ├── GMM_test.cpp │ ├── cmd_repub.cpp │ ├── cmd_test.cpp │ ├── fov_ray_test.cpp │ ├── swarm_data_test.cpp │ └── traj_swarm.cpp │ └── worlds │ ├── amagi1.world │ ├── building.world │ ├── edgar_mine.world │ ├── flat.world │ ├── flat3.world │ ├── forest.world │ ├── large.world │ ├── large_maze.world │ ├── large_maze_lid.world │ ├── map_xingxing.world │ ├── maze1.world │ ├── maze2.world │ ├── maze3.world │ ├── maze4.world │ ├── power_plant.world │ ├── testcity.world │ ├── testcity2.world │ ├── testmap.world │ └── tunnel.world ├── Trajectory ├── gcopter │ ├── CMakeLists.txt │ ├── config │ │ └── opt.yaml │ ├── include │ │ └── gcopter │ │ │ ├── fastcheck_traj.hpp │ │ │ ├── gcopter.hpp │ │ │ ├── geo_utils.hpp │ │ │ ├── lbfgs.hpp │ │ │ ├── minco.hpp │ │ │ ├── quickhull.hpp │ │ │ ├── root_finder.hpp │ │ │ ├── sdlp.hpp │ │ │ ├── traj_opt.h │ │ │ └── trajectory.hpp │ ├── package.xml │ └── src │ │ └── traj_opt.cpp ├── traj_exc │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ │ └── traj_exc_node.cpp └── yaw_planner │ ├── CMakeLists.txt │ ├── include │ └── yaw_planner │ │ └── yaw_planner.h │ ├── package.xml │ └── src │ └── yaw_planner.cpp ├── data_statistics ├── CMakeLists.txt ├── include │ └── data_statistics │ │ └── computation_statistician.h ├── launch │ ├── vt_statistician.launch │ └── vts_test.launch ├── package.xml ├── rviz │ └── vts.rviz └── src │ └── computation_statistician.cpp ├── doing ├── DTG-based-planning2.pdf ├── DTG-based-planning3.pdf ├── Frontier_communication.pdf ├── experiment.txt ├── local_voronoi_partition.pdf └── todo.txt └── pics ├── 0.gif ├── 1.gif ├── cover.png └── cover_new.png /Communication/multisim_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(multisim_gazebo) 3 | 4 | add_definitions(-std=c++14) 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(catkin_simple REQUIRED) 8 | catkin_simple(ALL_DEPS_REQUIRED) 9 | include_directories( 10 | ${catkin_INCLUDE_DIRS} 11 | ) 12 | 13 | cs_add_library(multi_rotors src/multi_rotors.cpp) 14 | 15 | cs_add_executable(multi_rotors_node src/multi_rotors_node.cpp) 16 | 17 | target_link_libraries(multi_rotors_node 18 | ${catkin_LIBRARIES} 19 | multi_rotors) 20 | 21 | ########## 22 | # EXPORT # 23 | ########## 24 | cs_install() 25 | cs_export() -------------------------------------------------------------------------------- /Communication/multisim_gazebo/include/multisim_gazebo/multi_rotors.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace std; 10 | class MultiRotors{ 11 | public: 12 | MultiRotors(){}; 13 | ~MultiRotors(){}; 14 | void init(ros::NodeHandle& nh, ros::NodeHandle& nh_private); 15 | private: 16 | void StatesCallback(const multisim_gazebo::statesConstPtr& msg); 17 | void OdomCallback(const nav_msgs::OdometryConstPtr& msg); 18 | void StatesTimerCallback(const ros::TimerEvent& e); 19 | 20 | // bool center_gazebo_; 21 | int robot_num_; 22 | int local_num_; 23 | double set_freq_; 24 | //local base link names 25 | vector local_blks_; 26 | //all model names 27 | vector all_mns_; 28 | vector have_odom_; 29 | 30 | vector local_ids_; 31 | vector odoms_; 32 | 33 | ros::NodeHandle nh_, nh_private_; 34 | 35 | vector odoms_sub_; 36 | vector odoms_show_pub_; 37 | ros::Publisher states_pub_; 38 | ros::Subscriber states_sub_; 39 | ros::Timer states_timer_; 40 | 41 | 42 | 43 | }; 44 | -------------------------------------------------------------------------------- /Communication/multisim_gazebo/launch/demo_2drones.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /Communication/multisim_gazebo/launch/rotors_hull_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /Communication/multisim_gazebo/launch/rotors_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /Communication/multisim_gazebo/msg/IdOdom.msg: -------------------------------------------------------------------------------- 1 | int8 id 2 | nav_msgs/Odometry odoms -------------------------------------------------------------------------------- /Communication/multisim_gazebo/msg/states.msg: -------------------------------------------------------------------------------- 1 | float64 stamp 2 | int8 from_computer 3 | multisim_gazebo/IdOdom[] odoms -------------------------------------------------------------------------------- /Communication/multisim_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multisim_gazebo 4 | 0.0.0 5 | The multisim_gazebo package 6 | 7 | 8 | 9 | 10 | CharlieDog 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | catkin 19 | catkin_simple 20 | roscpp 21 | tf 22 | std_msgs 23 | nav_msgs 24 | sensor_msgs 25 | message_generation 26 | message_runtime 27 | gazebo_msgs 28 | 29 | 30 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /Communication/multisim_gazebo/resource/2_1_rotors.yaml: -------------------------------------------------------------------------------- 1 | multirotors/robot_num: 2 2 | multirotors/local_num: 1 3 | multirotors/local_ids: [2] # > 1 4 | multirotors/basic_names: ["firefly2/odometry_sensor1"] 5 | multirotors/model_names: ["firefly1", "firefly2"] 6 | multirotors/set_freq: 20.0 -------------------------------------------------------------------------------- /Communication/multisim_gazebo/src/multi_rotors.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | // #include 4 | 5 | void MultiRotors::init(ros::NodeHandle& nh, ros::NodeHandle& nh_private){ 6 | nh_ = nh; 7 | nh_private_ = nh_private; 8 | string ns = ros::this_node::getName(); 9 | // nh_private_.param("/multirotors/is_center", center_gazebo_, false); 10 | nh_private_.param(ns + "/multirotors/robot_num", robot_num_, 1); 11 | nh_private_.param(ns + "/multirotors/local_num", local_num_, 1); 12 | nh_private_.param(ns + "/multirotors/local_ids", local_ids_, {1}); 13 | nh_private_.param(ns + "/multirotors/basic_names", local_blks_, {string("robot")}); 14 | nh_private_.param(ns + "/multirotors/model_names", all_mns_, {string("robot")}); 15 | nh_private_.param(ns + "/multirotors/set_freq", set_freq_, 20.0); 16 | odoms_.resize(robot_num_); 17 | odoms_sub_.resize(local_num_); 18 | have_odom_.resize(robot_num_, 0); 19 | odoms_show_pub_.resize(robot_num_); 20 | states_sub_ = nh.subscribe("/communication/states", 10, &MultiRotors::StatesCallback, this); 21 | states_pub_ = nh.advertise("/multisim_local/states", 50); 22 | 23 | for(int i = 0; i < local_num_; i++){ 24 | odoms_sub_[i] = nh.subscribe("/multisim_local/odom" + to_string(local_ids_[i]), 1, &MultiRotors::OdomCallback, this); 25 | } 26 | for(int i = 0; i < robot_num_; i++){ 27 | bool adv_flag = true; 28 | for(auto &id : local_ids_) if(id == i + 1) { 29 | adv_flag = false; 30 | break; 31 | } 32 | if(adv_flag) odoms_show_pub_[i] = nh.advertise("/multisim_swarm/odom" + to_string(i+1), 1); 33 | } 34 | states_timer_ = nh_private_.createTimer(ros::Duration(1/set_freq_), &MultiRotors::StatesTimerCallback, this); 35 | } 36 | 37 | void MultiRotors::StatesCallback(const multisim_gazebo::statesConstPtr& msg){ 38 | double dt = abs((msg->stamp - ros::WallTime::now().toSec())); 39 | // if(dt > 0.1){ 40 | // ROS_ERROR("from %d delay%f", msg->from_computer, dt); 41 | // } 42 | // else{ 43 | for(auto &odom : msg->odoms){ 44 | int o_idx = odom.id - 1; 45 | if(o_idx + 1 <= robot_num_){ 46 | odoms_[o_idx] = odom.odoms; 47 | have_odom_[o_idx] = 1; 48 | } 49 | else{ 50 | ROS_ERROR("unknown odom id: %d", o_idx + 1); 51 | } 52 | } 53 | // } 54 | } 55 | 56 | void MultiRotors::StatesTimerCallback(const ros::TimerEvent& e){ 57 | ros::WallTime cs = ros::WallTime::now(); 58 | multisim_gazebo::states msg; 59 | msg.odoms.resize(local_num_); 60 | for(int i = 0; i < local_num_; i++){ 61 | msg.odoms[i].odoms = odoms_[local_ids_[i] - 1]; 62 | msg.odoms[i].id = local_ids_[i]; 63 | } 64 | msg.stamp = ros::WallTime::now().toSec(); 65 | states_pub_.publish(msg); 66 | for(int i = 0; i < robot_num_; i++){ 67 | bool local_flag = false; 68 | for(auto &l_id : local_ids_){ 69 | if(l_id - 1 == i) { 70 | local_flag = true; 71 | break; 72 | } 73 | } 74 | if(local_flag || !have_odom_[i]) continue; 75 | gazebo_msgs::SetModelState state_srv; 76 | // int idx = local_ids_[i] - 1; 77 | state_srv.request.model_state.reference_frame = "world"; 78 | state_srv.request.model_state.model_name = all_mns_[i]; 79 | state_srv.request.model_state.pose = odoms_[i].pose.pose; 80 | state_srv.request.model_state.twist = odoms_[i].twist.twist; 81 | ros::service::call("/gazebo/set_model_state", state_srv); 82 | 83 | odoms_show_pub_[i].publish(odoms_[i]); 84 | } 85 | if((ros::WallTime::now() - cs).toSec() > 1/set_freq_) 86 | cout<<"call spent:"<<(ros::WallTime::now() - cs).toSec()<child_frame_id; 91 | int l_idx = 0; 92 | for(auto &blk : local_blks_){ 93 | if(blk == child_frame) break; 94 | l_idx++; 95 | } 96 | if(l_idx == local_ids_.size()) { 97 | ROS_ERROR("unknown frame:%s", child_frame.c_str()); 98 | return; 99 | } 100 | int o_idx = local_ids_[l_idx] - 1; 101 | have_odom_[o_idx] = 1; 102 | odoms_[o_idx] = *msg; 103 | } 104 | 105 | 106 | 107 | -------------------------------------------------------------------------------- /Communication/multisim_gazebo/src/multi_rotors_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | int main(int argc, char **argv){ 5 | ros::init(argc, argv, "block_test"); 6 | ros::NodeHandle nh, nh_private("~"); 7 | 8 | MultiRotors MRS_; 9 | MRS_.init(nh, nh_private); 10 | ros::spin(); 11 | return 0; 12 | } -------------------------------------------------------------------------------- /Communication/multisim_gazebo/worlds/edgar_mine.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | model://edgar_mine 9 | 10 | 11 | 12 | EARTH_WGS84 13 | 47.3667 14 | 8.5500 15 | 500.0 16 | 0 17 | 18 | 19 | 20 | 21 | quick 22 | 1000 23 | 1.3 24 | 25 | 26 | 0 27 | 0.2 28 | 100 29 | 0.001 30 | 31 | 32 | 0.01 33 | 1 34 | 100 35 | 0 0 -9.8 36 | 37 | 38 | -------------------------------------------------------------------------------- /Communication/multisim_gazebo/worlds/tunnel.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | 9 | model://sun 10 | 11 | 12 | model://tunnel 13 | 14 | 15 | 16 | EARTH_WGS84 17 | 47.3667 18 | 8.5500 19 | 500.0 20 | 0 21 | 22 | 23 | 24 | 25 | quick 26 | 1000 27 | 1.3 28 | 29 | 30 | 0 31 | 0.2 32 | 100 33 | 0.001 34 | 35 | 36 | 0.01 37 | 1 38 | 100 39 | 0 0 -9.8 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /Communication/swarm_bridge/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "${default}", 6 | "limitSymbolsToIncludedHeaders": false 7 | }, 8 | "includePath": [ 9 | "/home/lty/qt_ws/devel/include/**", 10 | "/home/lty/livox_ws/devel/include/**", 11 | "/home/lty/realsense_ros/devel/include/**", 12 | "/home/lty/space_planner/ego-planner/devel/include/**", 13 | "/home/lty/space_planner/Teach-Repeat-Replan/devel/include/**", 14 | "/home/lty/space_planner/3D-Sparse-Skeleton/devel/include/**", 15 | "/home/lty/exploration/vox_ws/devel/include/**", 16 | "/home/lty/planner_ws/devel/include/**", 17 | "/home/lty/realsense_ros_mod/devel/include/**", 18 | "/home/lty/mavros_ws/devel/include/**", 19 | "/home/lty/cv_bridge/install/include/**", 20 | "/opt/ros/melodic/include/**", 21 | "/home/lty/space_planner/ego-planner/src/planner/bspline_opt/include/**", 22 | "/home/lty/qt_ws/src/class1_ros_qt_demo/include/**", 23 | "/home/lty/planner_ws/src/comparison/include/**", 24 | "/home/lty/space_planner/Teach-Repeat-Replan/src/trr/utils/DecompRos/decomp_ros_utils/include/**", 25 | "/home/lty/planner_ws/src/NK-Planner/discrete_trajectory/include/**", 26 | "/home/lty/space_planner/ego-planner/src/planner/plan_manage/include/**", 27 | "/home/lty/exploration/vox_ws/src/voxblox/eigen_checks/include/**", 28 | "/home/lty/planner_ws/src/NK-Planner/grid_map/include/**", 29 | "/home/lty/mavros_ws/src/mavros/libmavconn/include/**", 30 | "/home/lty/exploration/vox_ws/src/voxblox/mav_comm/mav_msgs/include/**", 31 | "/home/lty/exploration/vox_ws/src/voxblox/mav_comm/mav_planning_msgs/include/**", 32 | "/home/lty/mavros_ws/src/mavros/mavros/include/**", 33 | "/home/lty/mavros_ws/src/mavros/mavros_msgs/include/**", 34 | "/home/lty/exploration/vox_ws/src/voxblox/minkindr/minkindr/include/**", 35 | "/home/lty/exploration/vox_ws/src/voxblox/minkindr_ros/minkindr_conversions/include/**", 36 | "/home/lty/mocap_ws/src/motion_capture_system/mocap_base/include/**", 37 | "/home/lty/mocap_ws/src/motion_capture_system/mocap_qualisys/include/**", 38 | "/home/lty/mocap_ws/src/motion_capture_system/mocap_vicon/include/**", 39 | "/home/lty/planner_ws/src/simulation/uav_simulator/mockamap/include/**", 40 | "/home/lty/planner_ws/src/simulation/uav_simulator/Utils/multi_map_server/include/**", 41 | "/home/lty/planner_ws/src/NK-Planner/nk_planner/include/**", 42 | "/home/lty/exploration/vox_ws/src/voxblox/numpy_eigen/include/**", 43 | "/home/lty/planner_ws/src/NK-Planner/path_plan/include/**", 44 | "/home/lty/space_planner/ego-planner/src/planner/path_searching/include/**", 45 | "/home/lty/space_planner/3D-Sparse-Skeleton/src/polygon_generation/include/**", 46 | "/home/lty/space_planner/Teach-Repeat-Replan/src/trr/polyhedron_generator/include/**", 47 | "/home/lty/planner_ws/src/simulation/uav_simulator/Utils/pose_utils/include/**", 48 | "/home/lty/planner_ws/src/simulation/uav_simulator/Utils/quadrotor_msgs/include/**", 49 | "/home/lty/realsense_ros/src/realsense-ros/realsense2_camera/include/**", 50 | "/home/lty/planner_ws/src/simulation/uav_simulator/so3_control/include/**", 51 | "/home/lty/planner_ws/src/simulation/uav_simulator/so3_quadrotor_simulator/include/**", 52 | "/home/lty/qt_ws/src/swarm_bridge/include/**", 53 | "/home/lty/mavros_ws/src/mavros/test_mavros/include/**", 54 | "/home/lty/planner_ws/src/controller/traj_ctrl/include/**", 55 | "/home/lty/planner_ws/src/NK-Planner/traj_opt/include/**", 56 | "/home/lty/qt_ws/src/traj_utils/include/**", 57 | "/home/lty/space_planner/Teach-Repeat-Replan/src/trr/global_planner/include/**", 58 | "/home/lty/planner_ws/src/simulation/uav_simulator/Utils/uav_utils/include/**", 59 | "/home/lty/exploration/vox_ws/src/voxblox/voxblox/voxblox/include/**", 60 | "/home/lty/exploration/vox_ws/src/voxblox/voxblox/voxblox_ros/include/**", 61 | "/home/lty/exploration/vox_ws/src/voxblox/voxblox/voxblox_rviz_plugin/include/**", 62 | "/usr/include/**" 63 | ], 64 | "name": "ROS", 65 | "intelliSenseMode": "gcc-x64", 66 | "compilerPath": "/usr/bin/gcc", 67 | "cStandard": "gnu11", 68 | "cppStandard": "c++14" 69 | } 70 | ], 71 | "version": 4 72 | } -------------------------------------------------------------------------------- /Communication/swarm_bridge/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/lty/qt_ws/devel/lib/python2.7/dist-packages", 4 | "/home/lty/livox_ws/devel/lib/python2.7/dist-packages", 5 | "/home/lty/realsense_ros/devel/lib/python2.7/dist-packages", 6 | "/home/lty/space_planner/ego-planner/devel/lib/python2.7/dist-packages", 7 | "/home/lty/space_planner/Teach-Repeat-Replan/devel/lib/python2.7/dist-packages", 8 | "/home/lty/space_planner/3D-Sparse-Skeleton/devel/lib/python2.7/dist-packages", 9 | "/home/lty/exploration/vox_ws/devel/lib/python2.7/dist-packages", 10 | "/home/lty/planner_ws/devel/lib/python2.7/dist-packages", 11 | "/home/lty/realsense_ros_mod/devel/lib/python2.7/dist-packages", 12 | "/home/lty/mavros_ws/devel/lib/python2.7/dist-packages", 13 | "/opt/ros/melodic/lib/python2.7/dist-packages" 14 | ], 15 | "files.associations": { 16 | "cctype": "cpp", 17 | "clocale": "cpp", 18 | "cmath": "cpp", 19 | "csignal": "cpp", 20 | "cstdarg": "cpp", 21 | "cstddef": "cpp", 22 | "cstdio": "cpp", 23 | "cstdlib": "cpp", 24 | "cstring": "cpp", 25 | "ctime": "cpp", 26 | "cwchar": "cpp", 27 | "cwctype": "cpp", 28 | "array": "cpp", 29 | "atomic": "cpp", 30 | "hash_map": "cpp", 31 | "hash_set": "cpp", 32 | "strstream": "cpp", 33 | "*.tcc": "cpp", 34 | "bitset": "cpp", 35 | "chrono": "cpp", 36 | "codecvt": "cpp", 37 | "complex": "cpp", 38 | "condition_variable": "cpp", 39 | "cstdint": "cpp", 40 | "deque": "cpp", 41 | "list": "cpp", 42 | "unordered_map": "cpp", 43 | "unordered_set": "cpp", 44 | "vector": "cpp", 45 | "exception": "cpp", 46 | "algorithm": "cpp", 47 | "functional": "cpp", 48 | "optional": "cpp", 49 | "ratio": "cpp", 50 | "string_view": "cpp", 51 | "system_error": "cpp", 52 | "tuple": "cpp", 53 | "type_traits": "cpp", 54 | "fstream": "cpp", 55 | "future": "cpp", 56 | "initializer_list": "cpp", 57 | "iomanip": "cpp", 58 | "iosfwd": "cpp", 59 | "iostream": "cpp", 60 | "istream": "cpp", 61 | "limits": "cpp", 62 | "memory": "cpp", 63 | "mutex": "cpp", 64 | "new": "cpp", 65 | "ostream": "cpp", 66 | "numeric": "cpp", 67 | "sstream": "cpp", 68 | "stdexcept": "cpp", 69 | "streambuf": "cpp", 70 | "thread": "cpp", 71 | "cfenv": "cpp", 72 | "cinttypes": "cpp", 73 | "utility": "cpp", 74 | "typeindex": "cpp", 75 | "typeinfo": "cpp", 76 | "iterator": "cpp", 77 | "map": "cpp", 78 | "memory_resource": "cpp", 79 | "random": "cpp", 80 | "set": "cpp", 81 | "string": "cpp" 82 | } 83 | } -------------------------------------------------------------------------------- /Communication/swarm_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(swarm_exp_bridge) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | ADD_COMPILE_OPTIONS(-std=c++11 ) 6 | ADD_COMPILE_OPTIONS(-std=c++14 ) 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | 11 | # requires zmq, zmqpp 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | std_msgs 16 | nav_msgs 17 | swarm_exp_msgs 18 | sensor_msgs 19 | geometry_msgs 20 | exp_comm_msgs 21 | multisim_gazebo 22 | gflags_catkin 23 | glog_catkin 24 | ) 25 | 26 | include_directories( 27 | SYSTEM 28 | ${catkin_INCLUDE_DIRS} 29 | ${PROJECT_SOURCE_DIR}/include 30 | ${EIGEN3_INCLUDE_DIR} 31 | ) 32 | 33 | 34 | 35 | catkin_package( 36 | CATKIN_DEPENDS 37 | ) 38 | 39 | 40 | # add_executable(bridge_ros_udp 41 | # src/bridge_ros_udp.cpp 42 | # ) 43 | # target_link_libraries(bridge_ros_udp 44 | # ${catkin_LIBRARIES} 45 | # ) 46 | 47 | add_executable(swarm_exp_bridge_tcp 48 | src/bridge_ros_tcp.cpp 49 | ) 50 | target_link_libraries(swarm_exp_bridge_tcp 51 | ${catkin_LIBRARIES} 52 | zmq 53 | zmqpp 54 | ) 55 | 56 | 57 | 58 | add_executable(swarm_exp_bridge_tcp_sim 59 | src/bridge_ros_tcp_sim.cpp 60 | ) 61 | target_link_libraries(swarm_exp_bridge_tcp_sim 62 | ${catkin_LIBRARIES} 63 | zmq 64 | zmqpp 65 | ) -------------------------------------------------------------------------------- /Communication/swarm_bridge/launch/local_tcp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /Communication/swarm_bridge/launch/test_tcp_drone2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /Communication/swarm_bridge/note.md: -------------------------------------------------------------------------------- 1 | # 计划列表 2 | 3 | ## 通信 4 | 5 | 1. 开发TCP的通信, 以及UDP的可视化通信 6 | 1. TCP：稳定且关键的数据传输, IP为本机IP 7 | - 机器人-机器人 8 | - Y 探索: frontier? + 地图?, include drone_id_ 9 | - Y 规划: 里程计 + 轨迹, include drone_id_ 10 | - 机器人-地面站 11 | - NONE 12 | - 地面站-机器人 13 | - Y 强制目标点, include drone_id_ 14 | - Y 是否遥控控制, include drone_id_ 15 | - Y 是否可视化展示, include drone_id_ 16 | - Y 是否急停, include drone_id_ 17 | 2. UDP：可视化信息展示, IP一定是XXX.XXX.XXX.255 18 | - 机器人-机器人 19 | - NONE 20 | - 机器人-地面站 21 | - 原始数据：压缩图像, include drone_id_ 22 | - 规划：里程计+轨迹, include drone_id_ 23 | - 探索：构建地图 + frontier, include drone_id_ 24 | - state 25 | - 地面站-机器人 26 | - 强制遥控控制, include drone_id_ 27 | 28 | ## 可视化 29 | 30 | 1. 展示 31 | 1. 探索+规划信息 32 | 1. RVIZ内展示 33 | 2. 无人机基础信息 34 | 1. 信号?电池?等 35 | 2. 交互 36 | 1. 多机快速选中 37 | 1. RVIZ多机选中 38 | 2. 强制目标(探索区域更新) 39 | 1. 3D GOAL 40 | 3. 第一视角展示 41 | 1. ？ 42 | 43 | 44 | 45 | sudo apt install libzmqpp-dev 46 | 47 | 48 | 49 | TODO1: add a new package for message -------------------------------------------------------------------------------- /Communication/swarm_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | swarm_exp_bridge 4 | 0.0.0 5 | The swarm_exp_bridge package 6 | 7 | 8 | 9 | 10 | iszhouxin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | std_msgs 54 | sensor_msgs 55 | nav_msgs 56 | geometry_msgs 57 | swarm_exp_msgs 58 | exp_comm_msgs 59 | multisim_gazebo 60 | 61 | roscpp 62 | std_msgs 63 | sensor_msgs 64 | nav_msgs 65 | geometry_msgs 66 | swarm_exp_msgs 67 | exp_comm_msgs 68 | multisim_gazebo 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /Communication/swarm_data/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(swarm_data) 3 | 4 | add_definitions(-std=c++14) 5 | set(CMAKE_BUILD_TYPE Release) 6 | find_package(Eigen3 REQUIRED) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_simple(ALL_DEPS_REQUIRED) 10 | 11 | include_directories( 12 | ${EIGEN3_INCLUDE_DIRS} 13 | ${catkin_INCLUDE_DIRS} 14 | ) 15 | 16 | cs_add_library(${PROJECT_NAME} src/swarm_data.cpp) 17 | 18 | # target_link_libraries(${PROJECT_NAME} 19 | # ${catkin_LIBRARIES}) 20 | ########## 21 | # EXPORT # 22 | ########## 23 | cs_install() 24 | cs_export() 25 | -------------------------------------------------------------------------------- /Communication/swarm_data/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | swarm_data 4 | 0.0.0 5 | The swarm_data package 6 | 7 | 8 | 9 | CharlieDog 10 | 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | catkin 18 | catkin_simple 19 | roscpp 20 | visualization_msgs 21 | sensor_msgs 22 | geometry_msgs 23 | nav_msgs 24 | gcopter 25 | swarm_exp_msgs 26 | exp_comm_msgs 27 | data_statistics 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /Exploration/graph_partition/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(graph_partition) 3 | 4 | add_definitions(-std=c++14) 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(catkin_simple REQUIRED) 8 | catkin_simple(ALL_DEPS_REQUIRED) 9 | 10 | include_directories( 11 | ${catkin_INCLUDE_DIRS} 12 | ) 13 | 14 | cs_add_library(${PROJECT_NAME} src/graph_partition.cpp) 15 | ########## 16 | # EXPORT # 17 | ########## 18 | cs_install() 19 | cs_export() -------------------------------------------------------------------------------- /Exploration/graph_partition/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | graph_partition 4 | 0.0.0 5 | The graph_partition package 6 | 7 | 8 | 9 | CharlieDog 10 | 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | catkin 18 | catkin_simple 19 | 20 | roscpp 21 | visualization_msgs 22 | 23 | sensor_msgs 24 | geometry_msgs 25 | block_map 26 | lowres_map 27 | frontier_grid 28 | swarm_data 29 | multiDTG 30 | yaw_planner 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /Exploration/multiDTG/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(multiDTG) 3 | 4 | add_definitions(-std=c++14) 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(catkin_simple REQUIRED) 8 | catkin_simple(ALL_DEPS_REQUIRED) 9 | 10 | include_directories( 11 | ${catkin_INCLUDE_DIRS} 12 | ) 13 | 14 | cs_add_library(${PROJECT_NAME} src/multiDTG.cpp src/multiDTG_path_search.cpp) 15 | ########## 16 | # EXPORT # 17 | ########## 18 | cs_install() 19 | cs_export() -------------------------------------------------------------------------------- /Exploration/multiDTG/include/multiDTG/dtg_structures.h: -------------------------------------------------------------------------------- 1 | #ifndef DTG_STRUCTURES_H_ 2 | #define DTG_STRUCTURES_H_ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | using namespace std; 13 | 14 | namespace DTG{ 15 | struct H_node; 16 | struct F_node; 17 | enum Fstate{ 18 | l_active, 19 | g_active, 20 | uncoverable 21 | }; 22 | 23 | enum Hstate{ 24 | L_ACTIVE, 25 | G_ACTIVE, 26 | L_FREE, 27 | BLOCKED 28 | }; 29 | 30 | template 31 | struct DTG_edge{ 32 | DTG_edge(){ 33 | length_ = 2e5; 34 | length_s_ = 1e5; 35 | head_ = 0; 36 | tail_ = 0; 37 | // head_s_ = 0; 38 | // tail_s_ = 0; 39 | e_flag_ = 0; 40 | } 41 | DTG_edge(shared_ptr &h, shared_ptr &t){ 42 | head_ = h->id_; 43 | // head_s_ = h->id_; 44 | tail_ = t->id_; 45 | // tail_s_ = t->id_; 46 | head_n_ = h; 47 | // head_n_s_ = h; 48 | tail_n_ = t; 49 | // tail_n_s_ = t; 50 | 51 | length_ = 2e5; 52 | length_s_ = 1e5; 53 | e_flag_ = 0; 54 | } 55 | double length_, length_s_; 56 | uint8_t e_flag_; //00(have global)(have local) (fake edge)(???)(showed)(checked) 57 | uint32_t head_, tail_; 58 | // uint32_t head_swarm_, tail_swarm_; 59 | list path_;//, path_swarm_; //head--->tail 60 | shared_ptr head_n_;//, head_n_s_; 61 | shared_ptr tail_n_;//, tail_n_s_; 62 | }; 63 | struct DTG_sch_node{ 64 | DTG_sch_node(const float &g, const float &f, const uint32_t &id, const Eigen::Vector3d &p){ 65 | g_ = g; 66 | f_ = f; 67 | id_ = id; 68 | pos_ = p; 69 | flag_ = 0; 70 | parent_ = NULL; 71 | } 72 | Eigen::Vector3d pos_; 73 | float g_, f_; 74 | uint32_t id_; 75 | uint8_t root_id_; 76 | uint8_t flag_; //0000 00(close)(h:1 f:0) 77 | shared_ptr parent_; 78 | }; 79 | 80 | class ACompare { 81 | public: 82 | bool operator()(shared_ptr node1, shared_ptr node2) { 83 | return node1->f_ > node2->f_; 84 | } 85 | }; 86 | 87 | class DCompare { 88 | public: 89 | bool operator()(shared_ptr node1, shared_ptr node2) { 90 | return node1->g_ > node2->g_; 91 | } 92 | }; 93 | 94 | typedef priority_queue, vector>, DCompare> prio_D; 95 | typedef priority_queue, vector>, ACompare> prio_A; 96 | 97 | struct F_node{ 98 | F_node(){ 99 | f_flag_ = 0; 100 | vp_id_ = -1; 101 | exploring_id_ = 0; 102 | } 103 | uint32_t id_; 104 | int vp_id_; 105 | Eigen::Vector3d center_; 106 | CoarseFrontier *cf_; 107 | uint8_t exploring_id_; 108 | // Eigen::Vector3d upbd_, lowbd_; 109 | float g_, h_; 110 | shared_ptr> hf_edge_; 111 | list>> edges_; 112 | // Fstate state_; 113 | uint8_t f_flag_; //0000 0(local gvp send)(local gvp)0 114 | shared_ptr sch_node_; 115 | }; 116 | 117 | 118 | struct H_node{ 119 | H_node(){ 120 | h_flags_ = 0; 121 | work_num_ = 0; 122 | } 123 | uint32_t id_; 124 | Eigen::Vector3d pos_; 125 | list>> hf_edges_; 126 | list>> hh_edges_; 127 | uint8_t work_num_; 128 | // uint8_t exploring_id_; 129 | // list> uav_dist_; 130 | Hstate state_; 131 | //for path search 132 | uint8_t h_flags_; //0000 0(global gvp)(local gvp)(close) 133 | shared_ptr sch_node_; 134 | }; 135 | } 136 | 137 | 138 | 139 | #endif -------------------------------------------------------------------------------- /Exploration/multiDTG/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multiDTG 4 | 0.0.0 5 | The multiDTG package 6 | 7 | 8 | 9 | CharlieDog 10 | 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | catkin 18 | catkin_simple 19 | 20 | roscpp 21 | visualization_msgs 22 | 23 | sensor_msgs 24 | geometry_msgs 25 | block_map 26 | lowres_map 27 | frontier_grid 28 | swarm_data 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /Exploration/murder_swarm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(murder_swarm) 3 | 4 | add_definitions(-std=c++14) 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(catkin_simple REQUIRED) 8 | catkin_simple(ALL_DEPS_REQUIRED) 9 | 10 | include_directories( 11 | ${catkin_INCLUDE_DIRS} 12 | ) 13 | 14 | cs_add_library(${PROJECT_NAME} src/murder.cpp) 15 | 16 | cs_add_library(${PROJECT_NAME}_fsm src/murderFSM.cpp) 17 | target_link_libraries(${PROJECT_NAME}_fsm 18 | ${catkin_LIBRARIES} ${PROJECT_NAME} ) 19 | 20 | cs_add_library(ground src/ground.cpp) 21 | 22 | 23 | cs_add_executable(murder_swarm_node src/murder_node.cpp) 24 | 25 | target_link_libraries(murder_swarm_node 26 | ${catkin_LIBRARIES} ${PROJECT_NAME}_fsm) 27 | 28 | cs_add_executable(swarm_trigger src/swarm_trigger.cpp) 29 | 30 | target_link_libraries(swarm_trigger 31 | ${catkin_LIBRARIES}) 32 | 33 | 34 | 35 | cs_add_executable(ground_node src/ground_node.cpp) 36 | 37 | target_link_libraries(ground_node 38 | ${catkin_LIBRARIES} ground) 39 | ########## 40 | # EXPORT # 41 | ########## 42 | cs_install() 43 | cs_export() -------------------------------------------------------------------------------- /Exploration/murder_swarm/include/murder_swarm/ground.h: -------------------------------------------------------------------------------- 1 | #ifndef GROUND_H_ 2 | #define GROUND_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | using namespace std; 27 | 28 | class Ground{ 29 | public: 30 | Ground(){}; 31 | ~Ground(){}; 32 | void init(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); 33 | private: 34 | void TrajVisCallback(const ros::TimerEvent &e); 35 | void PoseVisCallback(const ros::TimerEvent &e); 36 | void CreateVisModels(); 37 | 38 | inline void LoadVisModels(); 39 | 40 | ros::NodeHandle nh_, nh_private_; 41 | BlockMap BM_; 42 | DTG::MultiDTG MDTG_; 43 | AtoTraj TrajOpt_; 44 | FrontierGrid FG_; 45 | ColorManager CM_; 46 | lowres::LowResMap LRM_; 47 | SwarmDataManager SDM_; 48 | DTG::GraphVoronoiPartition GVP_; 49 | 50 | list> GVP_hn_; // self global partition, not used 51 | list> local_hn_; // self local partition, not used 52 | list local_fn_; // self exploring fn, not used 53 | 54 | ros::Timer traj_timer_, pose_timer_; 55 | ros::Publisher pose_vis_pub_, traj_vis_pub_; 56 | 57 | vector> traj_; 58 | vector poses_; 59 | visualization_msgs::MarkerArray vis_models_; 60 | }; 61 | 62 | 63 | inline void Ground::LoadVisModels(){ 64 | double cur_t = ros::WallTime::now().toSec(); 65 | for(int i = 0; i < SDM_.drone_num_; i++){ 66 | if(abs(cur_t - SDM_.Pose_t_[i]) < 0.2) poses_[i] = SDM_.Poses_[i]; 67 | vis_models_.markers[i*5 + 0].header.stamp = ros::Time::now(); 68 | vis_models_.markers[i*5 + 1].header.stamp = ros::Time::now(); 69 | vis_models_.markers[i*5 + 2].header.stamp = ros::Time::now(); 70 | vis_models_.markers[i*5 + 3].header.stamp = ros::Time::now(); 71 | vis_models_.markers[i*5 + 4].header.stamp = ros::Time::now(); 72 | 73 | vis_models_.markers[i*5 + 0].pose.orientation = poses_[i].orientation; 74 | vis_models_.markers[i*5 + 1].pose.orientation = poses_[i].orientation; 75 | vis_models_.markers[i*5 + 2].pose.orientation = poses_[i].orientation; 76 | vis_models_.markers[i*5 + 3].pose.orientation = poses_[i].orientation; 77 | vis_models_.markers[i*5 + 4].pose = poses_[i]; 78 | 79 | Eigen::Quaterniond rot; 80 | rot.x() = poses_[i].orientation.x; 81 | rot.y() = poses_[i].orientation.y; 82 | rot.z() = poses_[i].orientation.z; 83 | rot.w() = poses_[i].orientation.w; 84 | Eigen::Vector3d pos; 85 | pos(0) = poses_[i].position.x; 86 | pos(1) = poses_[i].position.y; 87 | pos(2) = poses_[i].position.z; 88 | 89 | Eigen::Vector3d p(0.225, 0.225, -0.02); 90 | vector pl; 91 | pl.emplace_back(p); 92 | p(0) = -p(0); 93 | pl.emplace_back(p); 94 | p(1) = -p(1); 95 | pl.emplace_back(p); 96 | p(0) = -p(0); 97 | pl.emplace_back(p); 98 | 99 | for(int j = 0; j < 4; j++){ 100 | p = rot.toRotationMatrix() * pl[j] + pos; 101 | vis_models_.markers[i*5 + j].pose.position.x = p(0); 102 | vis_models_.markers[i*5 + j].pose.position.y = p(1); 103 | vis_models_.markers[i*5 + j].pose.position.z = p(2); 104 | } 105 | } 106 | } 107 | #endif -------------------------------------------------------------------------------- /Exploration/murder_swarm/include/murder_swarm/murderFSM.h: -------------------------------------------------------------------------------- 1 | #ifndef MURDER_SWARM_FSM_H_ 2 | #define MURDER_SWARM_FSM_H_ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class MurderFSM{ 13 | public: 14 | /** 15 | * EXCUTE: excute traj, strongcheck several viewpoints, check the feasibility of the traj 16 | * SLEEP: before exploratin 17 | * FINISH: no explorable viewpoint, return start place 18 | * LOCALPLAN: to the closest local viewpoint 19 | * GLOBALPLAN: to the closest global viewpoint 20 | */ 21 | enum M_State{EXCUTE, SLEEP, FINISH, LOCALPLAN, GLOBALPLAN }; 22 | vector state_names = {"EXCUTE", "SLEEP", "FINISH", "LOCALPLAN", "GLOBALPLAN"}; 23 | MurderFSM(){}; 24 | ~MurderFSM(){}; 25 | void init(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); 26 | private: 27 | void FSMCallback(const ros::TimerEvent &e); 28 | void TriggerCallback(const std_msgs::EmptyConstPtr &msg); 29 | void ChangeState(const M_State &state); 30 | double fsm_min_duration_; 31 | double finish_t_, finish_hold_duration_; 32 | bool exploring_, start_trigger_; 33 | ros::Subscriber trigger_sub_; 34 | Murder M_planner_; 35 | ros::Timer fsm_timer_; 36 | M_State state_; 37 | double start_t_; 38 | }; 39 | #endif -------------------------------------------------------------------------------- /Exploration/murder_swarm/launch/debug.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /Exploration/murder_swarm/launch/murder_ground_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /Exploration/murder_swarm/launch/murder_single.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /Exploration/murder_swarm/launch/murder_swarm_maze3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | -------------------------------------------------------------------------------- /Exploration/murder_swarm/launch/murder_swarm_maze4.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | -------------------------------------------------------------------------------- /Exploration/murder_swarm/launch/murder_swarm_trigger.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /Exploration/murder_swarm/launch/rotors_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /Exploration/murder_swarm/launch/uav_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /Exploration/murder_swarm/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | murder_swarm 4 | 0.0.0 5 | The murder_swarm package 6 | 7 | 8 | 9 | CharlieDog 10 | 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | catkin 18 | catkin_simple 19 | 20 | roscpp 21 | visualization_msgs 22 | 23 | sensor_msgs 24 | swarm_exp_msgs 25 | geometry_msgs 26 | nav_msgs 27 | block_map 28 | swarm_data 29 | multiDTG 30 | lowres_map 31 | graph_partition 32 | gcopter 33 | yaw_planner 34 | frontier_grid 35 | gflags_catkin 36 | glog_catkin 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /Exploration/murder_swarm/resource/debug.yaml: -------------------------------------------------------------------------------- 1 | Frontier/sample_max_range: 3.0 2 | Frontier/grid_scale: 2.0 3 | Frontier/viewpoint_thresh: 1.2 4 | Frontier/observe_thresh: 0.85 5 | Frontier/resample_duration: 0.8 6 | Frontier/sample_hor_dir_num: 10 7 | Frontier/sample_ver_dir_num: 3 8 | Frontier/sample_dist_num: 2 9 | Frontier/sensor_type: "Depth_Camera" 10 | Frontier/samp_free_thresh: 15 11 | Frontier/FOV_hor_num: 15 12 | Frontier/FOV_ver_num: 10 13 | # Frontier/sensor_type: "Livox" 14 | # Frontier/FOV_hor_num: 35 15 | # Frontier/FOV_ver_num: 10 16 | Frontier/cam_hor: 1.9 17 | Frontier/cam_ver: 1.046 18 | Frontier/livox_ver_low: -0.174 19 | Frontier/livox_ver_up: 1.308 20 | Frontier/ray_samp_dist1: 0.2 21 | Frontier/ray_samp_dist2: 0.1 22 | Frontier/show_frontier: true 23 | 24 | block_map/minX: -22.5 25 | block_map/minY: -22.5 26 | block_map/minZ: -0.1 27 | block_map/maxX: 22.5 28 | block_map/maxY: 22.5 29 | block_map/maxZ: 3.5 30 | block_map/blockX: 10 31 | block_map/blockY: 10 32 | block_map/blockZ: 10 33 | block_map/resolution: 0.1 34 | block_map/sensor_max_range: 5.0 35 | block_map/FFD: false 36 | block_map/depth: true 37 | block_map/update_freq: 10.0 38 | block_map/show_freq: 10.0 39 | block_map/depth_step: 2 40 | block_map/CamtoBody_Quater_x: -0.5 41 | block_map/CamtoBody_Quater_y: 0.5 42 | block_map/CamtoBody_Quater_z: -0.5 43 | block_map/CamtoBody_Quater_w: 0.5 44 | block_map/CamtoBody_x: 0.0 45 | block_map/CamtoBody_y: 0.0 46 | block_map/CamtoBody_z: 0.0 47 | block_map/occ_max: 0.90 48 | block_map/occ_min: 0.20 49 | block_map/pro_hit_occ: 0.90 50 | block_map/pro_miss_free: 0.75 51 | 52 | EXP/finish_hold_duration: 15.0 53 | Exp/robot_sizeX: 0.5 54 | Exp/robot_sizeY: 0.5 55 | Exp/robot_sizeZ: 0.5 56 | Exp/minX: -20.0 57 | Exp/minY: -20.0 58 | Exp/minZ: 0.0 59 | Exp/maxX: 20.0 60 | Exp/maxY: 20.0 61 | Exp/maxZ: 3.0 62 | Exp/traj_length: 10.0 63 | Exp/local_path_search: 1000 64 | Exp/global_path_search: 3000 65 | Exp/strong_check_interval: 3.0 66 | Exp/check_duration: 3.5 67 | Exp/exc_duration: 0.5 68 | Exp/replan_duration: 1.5 69 | Exp/takeoff_x: 0.0 70 | Exp/takeoff_y: 1.0 71 | Exp/takeoff_z: 1.0 72 | Exp/takeoff_yaw: 0.0 73 | Exp/reach_out_t: 0.1 74 | Exp/RobotId: 3 75 | # Exp/UAV_id: 1 76 | Exp/AutoTakeoff: true 77 | 78 | GVD/show_gvd: true 79 | GVD/lambda: 0.1 80 | GVD/tau: 0.5 81 | GVD/partition_frequency: 0.33 82 | 83 | MR_DTG/Toporange: 10.0 84 | MR_DTG/H_thresh: 5.5 85 | MR_DTG/update_FF: false 86 | MR_DTG/resX: 2.0 87 | MR_DTG/resY: 2.0 88 | MR_DTG/resZ: 2.0 89 | MR_DTG/show_edge_details: false 90 | 91 | LowResMap/localgraph_sizex: 14.0 92 | LowResMap/localgraph_sizey: 14.0 93 | LowResMap/localgraph_sizez: 8.0 94 | LowResMap/node_x: 0.6 95 | LowResMap/node_y: 0.6 96 | LowResMap/node_z: 0.6 97 | LowResMap/blockX: 5 98 | LowResMap/blockY: 5 99 | LowResMap/blockZ: 3 100 | LowResMap/showmap: true 101 | LowResMap/debug: true 102 | LowResMap/lambda_heu: 5.0 103 | LowResMap/resolution: 0.1 104 | LowResMap/seg_length: 2.5 105 | LowResMap/prune_seg_length: 6.5 106 | LowResMap/occ_duration: 2.0 107 | LowResMap/unknown_duration: 0.2 108 | LowResMap/corridor_expX: 2 109 | LowResMap/corridor_expY: 2 110 | LowResMap/corridor_expZ: 1 111 | LowResMap/show_dtg: false 112 | 113 | /opt/YawVel: 2.0 114 | /opt/YawAcc: 2.0 115 | /opt/MaxVel: 1.5 116 | /opt/MaxAcc: 1.5 117 | /opt/WeiPos: 50000.0 118 | /opt/WeiVel: 10000.0 119 | /opt/WeiAcc: 10000.0 120 | /opt/WeiminT: 10000.0 121 | /opt/WeiT: 200.0 122 | /opt/smoothingEps: 0.01 123 | /opt/integralIntervs: 16 124 | /opt/RelCostTolL: 1.0e-5 -------------------------------------------------------------------------------- /Exploration/murder_swarm/resource/murder_maze3.yaml: -------------------------------------------------------------------------------- 1 | Frontier/sample_max_range: 2.2 2 | Frontier/grid_scale: 2.0 3 | Frontier/viewpoint_thresh: 1.3 4 | Frontier/observe_thresh: 0.85 5 | Frontier/resample_duration: 0.8 6 | Frontier/sample_hor_dir_num: 10 7 | Frontier/sample_ver_dir_num: 3 8 | Frontier/sample_dist_num: 2 9 | Frontier/sensor_type: "Depth_Camera" 10 | Frontier/samp_free_thresh: 15 11 | Frontier/FOV_hor_num: 15 12 | Frontier/FOV_ver_num: 10 13 | # Frontier/sensor_type: "Livox" 14 | # Frontier/FOV_hor_num: 35 15 | # Frontier/FOV_ver_num: 10 16 | Frontier/cam_hor: 1.9 17 | Frontier/cam_ver: 1.046 18 | Frontier/livox_ver_low: -0.174 19 | Frontier/livox_ver_up: 1.308 20 | Frontier/ray_samp_dist1: 0.2 21 | Frontier/ray_samp_dist2: 0.1 22 | Frontier/show_frontier: true 23 | Frontier/min_vp_num: 6 24 | 25 | block_map/RobotVisX: 0.81 26 | block_map/RobotVisY: 0.81 27 | block_map/RobotVisZ: 0.41 28 | block_map/swarm_tol: 0.3 29 | block_map/swarm_send_delay: 2.0 30 | block_map/swarm_pub_thresh: 0.975 31 | block_map/minX: -22.5 32 | block_map/minY: -22.5 33 | block_map/minZ: -0.1 34 | block_map/maxX: 22.5 35 | block_map/maxY: 22.5 36 | block_map/maxZ: 3.2 37 | block_map/blockX: 10 38 | block_map/blockY: 10 39 | block_map/blockZ: 10 40 | block_map/resolution: 0.1 41 | block_map/sensor_max_range: 5.0 42 | block_map/FFD: false 43 | block_map/depth: true 44 | block_map/update_freq: 10.0 45 | block_map/show_freq: 10.0 46 | block_map/depth_step: 2 47 | block_map/CamtoBody_Quater_x: -0.5 48 | block_map/CamtoBody_Quater_y: 0.5 49 | block_map/CamtoBody_Quater_z: -0.5 50 | block_map/CamtoBody_Quater_w: 0.5 51 | block_map/CamtoBody_x: 0.0 52 | block_map/CamtoBody_y: 0.0 53 | block_map/CamtoBody_z: 0.0 54 | block_map/occ_max: 0.90 55 | block_map/occ_min: 0.20 56 | block_map/pro_hit_occ: 0.90 57 | block_map/pro_miss_free: 0.75 58 | block_map/statistic_v: true 59 | block_map/show_block: true 60 | block_map/vis_mode: false 61 | block_map/min_finish_t: 50.0 62 | 63 | Exp/show_swarm_traj: false 64 | EXP/finish_hold_duration: 15.0 65 | Exp/robot_sizeX: 0.48 66 | Exp/robot_sizeY: 0.48 67 | Exp/robot_sizeZ: 0.3 68 | Exp/minX: -20.0 69 | Exp/minY: -20.0 70 | Exp/minZ: 0.0 71 | Exp/maxX: 20.0 72 | Exp/maxY: 20.0 73 | Exp/maxZ: 3.0 74 | Exp/traj_length: 10.0 75 | Exp/local_path_search: 1000 76 | Exp/global_path_search: 3000 77 | Exp/strong_check_interval: 3.0 78 | Exp/check_duration: 3.5 79 | Exp/exc_duration: 0.5 80 | Exp/replan_duration: 1.5 81 | Exp/takeoff_x: 0.0 82 | Exp/takeoff_y: 0.0 83 | Exp/takeoff_z: 1.0 84 | Exp/takeoff_yaw: 0.0 85 | Exp/reach_out_t: 0.1 86 | Exp/AutoTakeoff: true 87 | Exp/acc_off: -0.5 88 | Exp/statistic: true 89 | Exp/finish_thresh: 0.69 90 | Exp/swarm_check: true 91 | Exp/colli_range: 0.8 92 | 93 | #swarm 94 | # Exp/UAV_id: 1 95 | # Exp/drone_num: 1 96 | Exp/local_comm_freq: 5.0 97 | Exp/global_comm_freq: 0.5 98 | Exp/local_dist_thresh: 7.0 99 | 100 | GVD/show_gvd: true 101 | GVD/lambda: 0.1 102 | GVD/allowance: 0.08 103 | GVD/tau: 0.2 104 | GVD/partition_frequency: 0.33 105 | 106 | MR_DTG/Toporange: 10.0 107 | MR_DTG/H_thresh: 5.5 108 | MR_DTG/update_FF: false 109 | MR_DTG/resX: 2.0 110 | MR_DTG/resY: 2.0 111 | MR_DTG/resZ: 2.0 112 | MR_DTG/show_edge_details: true 113 | 114 | LowResMap/localgraph_sizex: 14.0 115 | LowResMap/localgraph_sizey: 14.0 116 | LowResMap/localgraph_sizez: 8.0 117 | LowResMap/node_x: 0.6 118 | LowResMap/node_y: 0.6 119 | LowResMap/node_z: 0.6 120 | LowResMap/blockX: 5 121 | LowResMap/blockY: 5 122 | LowResMap/blockZ: 3 123 | LowResMap/showmap: true 124 | LowResMap/debug: true 125 | LowResMap/lambda_heu: 5.0 126 | LowResMap/resolution: 0.1 127 | LowResMap/seg_length: 2.5 128 | LowResMap/prune_seg_length: 6.5 129 | LowResMap/occ_duration: 2.0 130 | LowResMap/unknown_duration: 0.2 131 | LowResMap/corridor_expX: 2 132 | LowResMap/corridor_expY: 2 133 | LowResMap/corridor_expZ: 1 134 | LowResMap/show_dtg: false 135 | 136 | opt/YawVel: 2.2 137 | opt/YawAcc: 2.2 138 | opt/MaxVel: 1.5 139 | opt/MaxAcc: 1.5 140 | opt/MaxJer: 25.0 141 | opt/SwarmAvoid: 1.2 142 | opt/WeiPos: 5000.0 143 | opt/WeiVel: 800.0 144 | opt/WeiAcc: 400.0 145 | opt/WeiJer: 50.0 146 | opt/WeiSwarm: 100.0 147 | opt/WeiJer: 50.0 148 | opt/WeiminT: 10.0 149 | opt/WeiT: 200.0 150 | opt/smoothingEps: 0.01 151 | opt/integralIntervs: 16 152 | opt/RelCostTolL: 1.0e-5 153 | 154 | Computation/data_num: 3 155 | Computation/dir: /home/charliedog/data/swarm/maze3/MURDER 156 | Computation/data_name0: gvp 157 | Computation/data_name1: local 158 | Computation/data_name2: global 159 | 160 | Computation/vdata_num: 9 161 | Computation/vdata_name0: volume 162 | Computation/vdata_name1: DTG 163 | Computation/vdata_name2: pose 164 | Computation/vdata_name3: traj 165 | Computation/vdata_name4: job 166 | Computation/vdata_name5: state 167 | Computation/vdata_name6: map 168 | Computation/vdata_name7: mapreq 169 | Computation/vdata_name8: total -------------------------------------------------------------------------------- /Exploration/murder_swarm/resource/murder_maze3_ground.yaml: -------------------------------------------------------------------------------- 1 | Frontier/sample_max_range: 3.0 2 | Frontier/grid_scale: 2.0 3 | Frontier/viewpoint_thresh: 1.0 4 | Frontier/observe_thresh: 0.85 5 | Frontier/resample_duration: 0.8 6 | Frontier/sample_hor_dir_num: 10 7 | Frontier/sample_ver_dir_num: 3 8 | Frontier/sample_dist_num: 2 9 | Frontier/sensor_type: "Depth_Camera" 10 | Frontier/samp_free_thresh: 15 11 | Frontier/FOV_hor_num: 15 12 | Frontier/FOV_ver_num: 10 13 | # Frontier/sensor_type: "Livox" 14 | # Frontier/FOV_hor_num: 35 15 | # Frontier/FOV_ver_num: 10 16 | Frontier/cam_hor: 1.9 17 | Frontier/cam_ver: 1.046 18 | Frontier/livox_ver_low: -0.174 19 | Frontier/livox_ver_up: 1.308 20 | Frontier/ray_samp_dist1: 0.2 21 | Frontier/ray_samp_dist2: 0.1 22 | Frontier/show_frontier: true 23 | Frontier/min_vp_num: 6 24 | 25 | 26 | block_map/RobotVisX: 0.81 27 | block_map/RobotVisY: 0.81 28 | block_map/RobotVisZ: 0.41 29 | block_map/swarm_tol: 0.3 30 | block_map/swarm_send_delay: 2.0 31 | block_map/swarm_pub_thresh: 0.98 32 | block_map/minX: -22.5 33 | block_map/minY: -22.5 34 | block_map/minZ: -0.1 35 | block_map/maxX: 22.5 36 | block_map/maxY: 22.5 37 | block_map/maxZ: 3.2 38 | block_map/blockX: 10 39 | block_map/blockY: 10 40 | block_map/blockZ: 10 41 | block_map/resolution: 0.1 42 | block_map/sensor_max_range: 5.0 43 | block_map/FFD: false 44 | block_map/depth: true 45 | block_map/update_freq: 10.0 46 | block_map/show_freq: 10.0 47 | block_map/depth_step: 2 48 | block_map/CamtoBody_Quater_x: -0.5 49 | block_map/CamtoBody_Quater_y: 0.5 50 | block_map/CamtoBody_Quater_z: -0.5 51 | block_map/CamtoBody_Quater_w: 0.5 52 | block_map/CamtoBody_x: 0.0 53 | block_map/CamtoBody_y: 0.0 54 | block_map/CamtoBody_z: 0.0 55 | block_map/occ_max: 0.90 56 | block_map/occ_min: 0.20 57 | block_map/pro_hit_occ: 0.90 58 | block_map/pro_miss_free: 0.75 59 | block_map/statistic_v: true 60 | block_map/show_block: true 61 | block_map/vis_mode: false 62 | block_map/min_finish_t: 50.0 63 | 64 | Exp/show_swarm_traj: false 65 | EXP/finish_hold_duration: 15.0 66 | Exp/robot_sizeX: 0.5 67 | Exp/robot_sizeY: 0.5 68 | Exp/robot_sizeZ: 0.5 69 | Exp/minX: -20.0 70 | Exp/minY: -20.0 71 | Exp/minZ: 0.0 72 | Exp/maxX: 20.0 73 | Exp/maxY: 20.0 74 | Exp/maxZ: 3.0 75 | Exp/traj_length: 10.0 76 | Exp/local_path_search: 1000 77 | Exp/global_path_search: 3000 78 | Exp/strong_check_interval: 3.0 79 | Exp/check_duration: 3.5 80 | Exp/exc_duration: 0.5 81 | Exp/replan_duration: 1.5 82 | Exp/takeoff_x: 0.0 83 | Exp/takeoff_y: 0.0 84 | Exp/takeoff_z: 1.0 85 | Exp/takeoff_yaw: 0.0 86 | Exp/reach_out_t: 0.1 87 | Exp/AutoTakeoff: true 88 | Exp/statistic: true 89 | Exp/finish_thresh: 0.5 90 | 91 | #swarm 92 | # Exp/UAV_id: 1 93 | # Exp/drone_num: 1 94 | Exp/local_comm_freq: 5.0 95 | Exp/global_comm_freq: 0.5 96 | Exp/local_dist_thresh: 7.0 97 | 98 | GVD/show_gvd: true 99 | GVD/lambda: 0.1 100 | GVD/allowance: 0.2 101 | GVD/tau: 0.5 102 | GVD/partition_frequency: 1.0 103 | 104 | MR_DTG/Toporange: 10.0 105 | MR_DTG/H_thresh: 5.5 106 | MR_DTG/update_FF: false 107 | MR_DTG/resX: 2.0 108 | MR_DTG/resY: 2.0 109 | MR_DTG/resZ: 2.0 110 | MR_DTG/show_edge_details: true 111 | 112 | LowResMap/localgraph_sizex: 14.0 113 | LowResMap/localgraph_sizey: 14.0 114 | LowResMap/localgraph_sizez: 8.0 115 | LowResMap/node_x: 0.6 116 | LowResMap/node_y: 0.6 117 | LowResMap/node_z: 0.6 118 | LowResMap/blockX: 5 119 | LowResMap/blockY: 5 120 | LowResMap/blockZ: 3 121 | LowResMap/showmap: true 122 | LowResMap/debug: true 123 | LowResMap/lambda_heu: 5.0 124 | LowResMap/resolution: 0.1 125 | LowResMap/seg_length: 2.5 126 | LowResMap/prune_seg_length: 6.5 127 | LowResMap/occ_duration: 2.0 128 | LowResMap/unknown_duration: 0.2 129 | LowResMap/corridor_expX: 2 130 | LowResMap/corridor_expY: 2 131 | LowResMap/corridor_expZ: 1 132 | LowResMap/show_dtg: false 133 | 134 | opt/YawVel: 1.8 135 | opt/YawAcc: 1.8 136 | opt/MaxVel: 1.5 137 | opt/MaxAcc: 1.5 138 | opt/SwarmAvoid: 0.9 139 | opt/WeiPos: 5000.0 140 | opt/WeiVel: 500.0 141 | opt/WeiAcc: 100.0 142 | opt/WeiJer: 50.0 143 | opt/WeiSwarm: 300.0 144 | opt/WeiminT: 10.0 145 | opt/WeiT: 200.0 146 | opt/smoothingEps: 0.01 147 | opt/integralIntervs: 16 148 | opt/RelCostTolL: 1.0e-5 149 | 150 | Computation/data_num: 3 151 | Computation/dir: /home/charliedog/data/swarm/maze3/MURDER 152 | Computation/data_name0: gvp 153 | Computation/data_name1: local 154 | Computation/data_name2: global 155 | 156 | Computation/vdata_num: 9 157 | Computation/vdata_name0: volume 158 | Computation/vdata_name1: DTG 159 | Computation/vdata_name2: pose 160 | Computation/vdata_name3: traj 161 | Computation/vdata_name4: job 162 | Computation/vdata_name5: state 163 | Computation/vdata_name6: map 164 | Computation/vdata_name7: mapreq 165 | Computation/vdata_name8: total -------------------------------------------------------------------------------- /Exploration/murder_swarm/resource/murder_maze4.yaml: -------------------------------------------------------------------------------- 1 | Frontier/sample_max_range: 2.2 2 | Frontier/grid_scale: 2.0 3 | Frontier/viewpoint_thresh: 1.3 4 | Frontier/observe_thresh: 0.85 5 | Frontier/resample_duration: 0.8 6 | Frontier/sample_hor_dir_num: 10 7 | Frontier/sample_ver_dir_num: 3 8 | Frontier/sample_dist_num: 2 9 | 10 | Frontier/sensor_type: "Depth_Camera" 11 | Frontier/FOV_hor_num: 15 12 | Frontier/FOV_ver_num: 10 13 | Frontier/cam_hor: 1.9 14 | Frontier/cam_ver: 1.046 15 | 16 | # Frontier/sensor_type: "Livox" 17 | # Frontier/FOV_hor_num: 35 18 | # Frontier/FOV_ver_num: 10 19 | # Frontier/livox_ver_up: 1.308 20 | # Frontier/livox_ver_low: -0.175 21 | 22 | Frontier/ray_samp_dist1: 0.2 23 | Frontier/ray_samp_dist2: 0.1 24 | Frontier/show_frontier: true 25 | Frontier/min_vp_num: 6 26 | 27 | block_map/RobotVisX: 0.81 28 | block_map/RobotVisY: 0.81 29 | block_map/RobotVisZ: 0.41 30 | block_map/swarm_tol: 0.3 31 | block_map/swarm_send_delay: 2.0 32 | block_map/swarm_pub_thresh: 0.975 33 | block_map/minX: -22.5 34 | block_map/minY: -22.5 35 | block_map/minZ: -0.1 36 | block_map/maxX: 22.5 37 | block_map/maxY: 22.5 38 | block_map/maxZ: 3.2 39 | block_map/blockX: 10 40 | block_map/blockY: 10 41 | block_map/blockZ: 10 42 | block_map/resolution: 0.1 43 | block_map/sensor_max_range: 5.0 44 | block_map/FFD: false 45 | block_map/depth: true 46 | block_map/update_freq: 10.0 47 | block_map/show_freq: 10.0 48 | block_map/depth_step: 2 49 | block_map/CamtoBody_Quater_x: -0.5 50 | block_map/CamtoBody_Quater_y: 0.5 51 | block_map/CamtoBody_Quater_z: -0.5 52 | block_map/CamtoBody_Quater_w: 0.5 53 | block_map/CamtoBody_x: 0.0 54 | block_map/CamtoBody_y: 0.0 55 | block_map/CamtoBody_z: 0.0 56 | block_map/occ_max: 0.90 57 | block_map/occ_min: 0.20 58 | block_map/pro_hit_occ: 0.90 59 | block_map/pro_miss_free: 0.75 60 | block_map/statistic_v: true 61 | block_map/show_block: true 62 | block_map/vis_mode: false 63 | block_map/min_finish_t: 45.0 64 | 65 | Exp/show_swarm_traj: false 66 | EXP/finish_hold_duration: 15.0 67 | Exp/robot_sizeX: 0.48 68 | Exp/robot_sizeY: 0.48 69 | Exp/robot_sizeZ: 0.3 70 | Exp/minX: -10.0 71 | Exp/minY: -20.0 72 | Exp/minZ: 0.0 73 | Exp/maxX: 10.0 74 | Exp/maxY: 20.0 75 | Exp/maxZ: 3.0 76 | Exp/traj_length: 10.0 77 | Exp/local_path_search: 1000 78 | Exp/global_path_search: 3000 79 | Exp/strong_check_interval: 3.0 80 | Exp/check_duration: 3.5 81 | Exp/exc_duration: 0.5 82 | Exp/replan_duration: 1.5 83 | Exp/takeoff_x: 0.0 84 | Exp/takeoff_y: 0.0 85 | Exp/takeoff_z: 1.0 86 | Exp/takeoff_yaw: 0.0 87 | Exp/reach_out_t: 0.1 88 | Exp/AutoTakeoff: true 89 | Exp/acc_off: -0.5 90 | Exp/statistic: true 91 | Exp/finish_thresh: 0.5 92 | Exp/swarm_check: true 93 | Exp/colli_range: 0.8 94 | #swarm 95 | # Exp/UAV_id: 1 96 | # Exp/drone_num: 1 97 | Exp/local_comm_freq: 5.0 98 | Exp/global_comm_freq: 0.5 99 | Exp/local_dist_thresh: 7.0 100 | 101 | GVD/show_gvd: true 102 | GVD/lambda: 0.1 103 | GVD/allowance: 0.08 104 | GVD/tau: 0.2 105 | GVD/partition_frequency: 0.33 106 | 107 | MR_DTG/Toporange: 10.0 108 | MR_DTG/H_thresh: 5.5 109 | MR_DTG/update_FF: false 110 | MR_DTG/resX: 2.0 111 | MR_DTG/resY: 2.0 112 | MR_DTG/resZ: 2.0 113 | MR_DTG/show_edge_details: false 114 | 115 | LowResMap/localgraph_sizex: 14.0 116 | LowResMap/localgraph_sizey: 14.0 117 | LowResMap/localgraph_sizez: 8.0 118 | LowResMap/node_x: 0.6 119 | LowResMap/node_y: 0.6 120 | LowResMap/node_z: 0.6 121 | LowResMap/blockX: 5 122 | LowResMap/blockY: 5 123 | LowResMap/blockZ: 3 124 | LowResMap/showmap: true 125 | LowResMap/debug: true 126 | LowResMap/lambda_heu: 5.0 127 | LowResMap/resolution: 0.1 128 | LowResMap/seg_length: 2.5 129 | LowResMap/prune_seg_length: 6.5 130 | LowResMap/occ_duration: 2.0 131 | LowResMap/unknown_duration: 0.2 132 | LowResMap/corridor_expX: 2 133 | LowResMap/corridor_expY: 2 134 | LowResMap/corridor_expZ: 1 135 | LowResMap/show_dtg: false 136 | 137 | opt/YawVel: 2.2 138 | opt/YawAcc: 2.2 139 | opt/MaxVel: 1.5 140 | opt/MaxAcc: 1.5 141 | opt/MaxJer: 25.0 142 | opt/SwarmAvoid: 1.2 143 | opt/WeiPos: 5000.0 144 | opt/WeiVel: 800.0 145 | opt/WeiAcc: 400.0 146 | opt/WeiJer: 50.0 147 | opt/WeiSwarm: 100.0 148 | opt/WeiminT: 10.0 149 | opt/WeiT: 200.0 150 | opt/smoothingEps: 0.01 151 | opt/integralIntervs: 16 152 | opt/RelCostTolL: 1.0e-5 153 | 154 | Computation/data_num: 3 155 | Computation/dir: /home/charliedog/data/swarm/maze4/MURDER 156 | Computation/data_name0: gvp 157 | Computation/data_name1: local 158 | Computation/data_name2: global 159 | 160 | Computation/vdata_num: 9 161 | Computation/vdata_name0: volume 162 | Computation/vdata_name1: DTG 163 | Computation/vdata_name2: pose 164 | Computation/vdata_name3: traj 165 | Computation/vdata_name4: job 166 | Computation/vdata_name5: state 167 | Computation/vdata_name6: map 168 | Computation/vdata_name7: mapreq 169 | Computation/vdata_name8: total -------------------------------------------------------------------------------- /Exploration/murder_swarm/resource/murder_maze4_ground.yaml: -------------------------------------------------------------------------------- 1 | Frontier/sample_max_range: 3.0 2 | Frontier/grid_scale: 2.0 3 | Frontier/viewpoint_thresh: 1.3 4 | Frontier/observe_thresh: 0.85 5 | Frontier/resample_duration: 0.8 6 | Frontier/sample_hor_dir_num: 10 7 | Frontier/sample_ver_dir_num: 3 8 | Frontier/sample_dist_num: 2 9 | Frontier/sensor_type: "Depth_Camera" 10 | Frontier/samp_free_thresh: 15 11 | Frontier/FOV_hor_num: 15 12 | Frontier/FOV_ver_num: 10 13 | # Frontier/sensor_type: "Livox" 14 | # Frontier/FOV_hor_num: 35 15 | # Frontier/FOV_ver_num: 10 16 | Frontier/cam_hor: 1.9 17 | Frontier/cam_ver: 1.046 18 | Frontier/livox_ver_low: -0.174 19 | Frontier/livox_ver_up: 1.308 20 | Frontier/ray_samp_dist1: 0.2 21 | Frontier/ray_samp_dist2: 0.1 22 | Frontier/show_frontier: true 23 | Frontier/min_vp_num: 6 24 | 25 | 26 | block_map/RobotVisX: 0.81 27 | block_map/RobotVisY: 0.81 28 | block_map/RobotVisZ: 0.41 29 | block_map/swarm_tol: 0.3 30 | block_map/swarm_send_delay: 2.0 31 | block_map/swarm_pub_thresh: 0.97 32 | block_map/minX: -22.5 33 | block_map/minY: -22.5 34 | block_map/minZ: -0.1 35 | block_map/maxX: 22.5 36 | block_map/maxY: 22.5 37 | block_map/maxZ: 3.2 38 | block_map/blockX: 10 39 | block_map/blockY: 10 40 | block_map/blockZ: 10 41 | block_map/resolution: 0.1 42 | block_map/sensor_max_range: 5.0 43 | block_map/FFD: false 44 | block_map/depth: true 45 | block_map/update_freq: 10.0 46 | block_map/show_freq: 10.0 47 | block_map/depth_step: 2 48 | block_map/CamtoBody_Quater_x: -0.5 49 | block_map/CamtoBody_Quater_y: 0.5 50 | block_map/CamtoBody_Quater_z: -0.5 51 | block_map/CamtoBody_Quater_w: 0.5 52 | block_map/CamtoBody_x: 0.0 53 | block_map/CamtoBody_y: 0.0 54 | block_map/CamtoBody_z: 0.0 55 | block_map/occ_max: 0.90 56 | block_map/occ_min: 0.20 57 | block_map/pro_hit_occ: 0.90 58 | block_map/pro_miss_free: 0.75 59 | block_map/statistic_v: true 60 | block_map/show_block: true 61 | block_map/vis_mode: false 62 | block_map/min_finish_t: 30.0 63 | 64 | Exp/show_swarm_traj: false 65 | EXP/finish_hold_duration: 15.0 66 | Exp/robot_sizeX: 0.5 67 | Exp/robot_sizeY: 0.5 68 | Exp/robot_sizeZ: 0.5 69 | Exp/minX: -10.0 70 | Exp/minY: -20.0 71 | Exp/minZ: 0.0 72 | Exp/maxX: 10.0 73 | Exp/maxY: 20.0 74 | Exp/maxZ: 3.0 75 | Exp/traj_length: 10.0 76 | Exp/local_path_search: 1000 77 | Exp/global_path_search: 3000 78 | Exp/strong_check_interval: 3.0 79 | Exp/check_duration: 3.5 80 | Exp/exc_duration: 0.5 81 | Exp/replan_duration: 1.5 82 | Exp/takeoff_x: 0.0 83 | Exp/takeoff_y: 0.0 84 | Exp/takeoff_z: 1.0 85 | Exp/takeoff_yaw: 0.0 86 | Exp/reach_out_t: 0.1 87 | Exp/AutoTakeoff: true 88 | Exp/statistic: true 89 | Exp/finish_thresh: 0.5 90 | 91 | #swarm 92 | # Exp/UAV_id: 1 93 | # Exp/drone_num: 1 94 | Exp/local_comm_freq: 5.0 95 | Exp/global_comm_freq: 0.5 96 | Exp/local_dist_thresh: 7.0 97 | 98 | GVD/show_gvd: true 99 | GVD/lambda: 0.1 100 | GVD/allowance: 0.2 101 | GVD/tau: 0.5 102 | GVD/partition_frequency: 1.0 103 | 104 | MR_DTG/Toporange: 10.0 105 | MR_DTG/H_thresh: 5.5 106 | MR_DTG/update_FF: false 107 | MR_DTG/resX: 2.0 108 | MR_DTG/resY: 2.0 109 | MR_DTG/resZ: 2.0 110 | MR_DTG/show_edge_details: false 111 | 112 | LowResMap/localgraph_sizex: 14.0 113 | LowResMap/localgraph_sizey: 14.0 114 | LowResMap/localgraph_sizez: 8.0 115 | LowResMap/node_x: 0.6 116 | LowResMap/node_y: 0.6 117 | LowResMap/node_z: 0.6 118 | LowResMap/blockX: 5 119 | LowResMap/blockY: 5 120 | LowResMap/blockZ: 3 121 | LowResMap/showmap: true 122 | LowResMap/debug: true 123 | LowResMap/lambda_heu: 5.0 124 | LowResMap/resolution: 0.1 125 | LowResMap/seg_length: 2.5 126 | LowResMap/prune_seg_length: 6.5 127 | LowResMap/occ_duration: 2.0 128 | LowResMap/unknown_duration: 0.2 129 | LowResMap/corridor_expX: 2 130 | LowResMap/corridor_expY: 2 131 | LowResMap/corridor_expZ: 1 132 | LowResMap/show_dtg: false 133 | 134 | opt/YawVel: 1.8 135 | opt/YawAcc: 1.8 136 | opt/MaxVel: 1.5 137 | opt/MaxAcc: 1.5 138 | opt/MaxJer: 15.0 139 | opt/SwarmAvoid: 0.9 140 | opt/WeiPos: 5000.0 141 | opt/WeiVel: 500.0 142 | opt/WeiAcc: 100.0 143 | opt/WeiJer: 50.0 144 | opt/WeiSwarm: 500.0 145 | opt/WeiminT: 10.0 146 | opt/WeiT: 200.0 147 | opt/smoothingEps: 0.01 148 | opt/integralIntervs: 16 149 | opt/RelCostTolL: 1.0e-5 150 | 151 | Computation/data_num: 3 152 | Computation/dir: /home/charliedog/data/swarm/maze4/MURDER 153 | Computation/data_name0: gvp 154 | Computation/data_name1: local 155 | Computation/data_name2: global 156 | 157 | Computation/vdata_num: 9 158 | Computation/vdata_name0: volume 159 | Computation/vdata_name1: DTG 160 | Computation/vdata_name2: pose 161 | Computation/vdata_name3: traj 162 | Computation/vdata_name4: job 163 | Computation/vdata_name5: state 164 | Computation/vdata_name6: map 165 | Computation/vdata_name7: mapreq 166 | Computation/vdata_name8: total -------------------------------------------------------------------------------- /Exploration/murder_swarm/resource/murder_swarm2_demo.yaml: -------------------------------------------------------------------------------- 1 | Frontier/sample_max_range: 3.0 2 | Frontier/grid_scale: 2.0 3 | Frontier/viewpoint_thresh: 0.9 4 | Frontier/observe_thresh: 0.85 5 | Frontier/resample_duration: 0.8 6 | Frontier/sample_hor_dir_num: 10 7 | Frontier/sample_ver_dir_num: 3 8 | Frontier/sample_dist_num: 2 9 | Frontier/sensor_type: "Depth_Camera" 10 | Frontier/samp_free_thresh: 15 11 | Frontier/FOV_hor_num: 15 12 | Frontier/FOV_ver_num: 10 13 | # Frontier/sensor_type: "Livox" 14 | # Frontier/FOV_hor_num: 35 15 | # Frontier/FOV_ver_num: 10 16 | Frontier/cam_hor: 1.9 17 | Frontier/cam_ver: 1.046 18 | Frontier/livox_ver_low: -0.174 19 | Frontier/livox_ver_up: 1.308 20 | Frontier/ray_samp_dist1: 0.2 21 | Frontier/ray_samp_dist2: 0.1 22 | Frontier/show_frontier: true 23 | 24 | block_map/RobotVisX: 0.81 25 | block_map/RobotVisY: 0.81 26 | block_map/RobotVisZ: 0.41 27 | block_map/minX: -22.5 28 | block_map/minY: -22.5 29 | block_map/minZ: -0.1 30 | block_map/maxX: 22.5 31 | block_map/maxY: 22.5 32 | block_map/maxZ: 3.5 33 | block_map/blockX: 10 34 | block_map/blockY: 10 35 | block_map/blockZ: 10 36 | block_map/resolution: 0.1 37 | block_map/sensor_max_range: 5.0 38 | block_map/FFD: false 39 | block_map/depth: true 40 | block_map/update_freq: 10.0 41 | block_map/show_freq: 10.0 42 | block_map/depth_step: 2 43 | block_map/CamtoBody_Quater_x: -0.5 44 | block_map/CamtoBody_Quater_y: 0.5 45 | block_map/CamtoBody_Quater_z: -0.5 46 | block_map/CamtoBody_Quater_w: 0.5 47 | block_map/CamtoBody_x: 0.0 48 | block_map/CamtoBody_y: 0.0 49 | block_map/CamtoBody_z: 0.0 50 | block_map/occ_max: 0.90 51 | block_map/occ_min: 0.20 52 | block_map/pro_hit_occ: 0.90 53 | block_map/pro_miss_free: 0.75 54 | block_map/statistic_v_: true 55 | 56 | EXP/finish_hold_duration: 15.0 57 | Exp/robot_sizeX: 0.48 58 | Exp/robot_sizeY: 0.48 59 | Exp/robot_sizeZ: 0.3 60 | Exp/minX: -20.0 61 | Exp/minY: -20.0 62 | Exp/minZ: 0.0 63 | Exp/maxX: 20.0 64 | Exp/maxY: 20.0 65 | Exp/maxZ: 3.0 66 | Exp/traj_length: 10.0 67 | Exp/local_path_search: 1000 68 | Exp/global_path_search: 3000 69 | Exp/strong_check_interval: 3.0 70 | Exp/check_duration: 3.5 71 | Exp/exc_duration: 0.5 72 | Exp/replan_duration: 1.5 73 | Exp/takeoff_x: 0.0 74 | Exp/takeoff_y: 1.0 75 | Exp/takeoff_z: 1.0 76 | Exp/takeoff_yaw: 0.0 77 | Exp/reach_out_t: 0.1 78 | Exp/AutoTakeoff: false 79 | 80 | #swarm 81 | # Exp/UAV_id: 2 82 | # Exp/drone_num: 2 83 | Exp/local_comm_freq: 5.0 84 | Exp/global_comm_freq: 0.5 85 | Exp/local_dist_thresh: 7.0 86 | 87 | GVD/show_gvd: true 88 | GVD/lambda: 0.1 89 | GVD/tau: 0.5 90 | GVD/partition_frequency: 0.33 91 | 92 | MR_DTG/Toporange: 10.0 93 | MR_DTG/H_thresh: 5.5 94 | MR_DTG/update_FF: false 95 | MR_DTG/resX: 2.0 96 | MR_DTG/resY: 2.0 97 | MR_DTG/resZ: 2.0 98 | MR_DTG/show_edge_details: true 99 | 100 | LowResMap/localgraph_sizex: 14.0 101 | LowResMap/localgraph_sizey: 14.0 102 | LowResMap/localgraph_sizez: 8.0 103 | LowResMap/node_x: 0.6 104 | LowResMap/node_y: 0.6 105 | LowResMap/node_z: 0.6 106 | LowResMap/blockX: 5 107 | LowResMap/blockY: 5 108 | LowResMap/blockZ: 3 109 | LowResMap/showmap: true 110 | LowResMap/debug: true 111 | LowResMap/lambda_heu: 5.0 112 | LowResMap/resolution: 0.1 113 | LowResMap/seg_length: 2.5 114 | LowResMap/prune_seg_length: 6.5 115 | LowResMap/occ_duration: 2.0 116 | LowResMap/unknown_duration: 0.2 117 | LowResMap/corridor_expX: 2 118 | LowResMap/corridor_expY: 2 119 | LowResMap/corridor_expZ: 1 120 | LowResMap/show_dtg: false 121 | 122 | opt/YawVel: 2.0 123 | opt/YawAcc: 2.0 124 | opt/MaxVel: 1.5 125 | opt/MaxAcc: 1.5 126 | opt/WeiPos: 50000.0 127 | opt/WeiVel: 10000.0 128 | opt/WeiAcc: 10000.0 129 | opt/WeiminT: 10000.0 130 | opt/WeiT: 200.0 131 | opt/smoothingEps: 0.01 132 | opt/integralIntervs: 16 133 | opt/RelCostTolL: 1.0e-5 -------------------------------------------------------------------------------- /Exploration/murder_swarm/resource/murder_testcity.yaml: -------------------------------------------------------------------------------- 1 | Frontier/sample_max_range: 2.2 2 | Frontier/grid_scale: 2.0 3 | Frontier/viewpoint_thresh: 1.3 4 | Frontier/observe_thresh: 0.65 5 | Frontier/resample_duration: 0.8 6 | Frontier/sample_hor_dir_num: 10 7 | Frontier/sample_ver_dir_num: 3 8 | Frontier/sample_dist_num: 2 9 | 10 | Frontier/sensor_type: "Depth_Camera" 11 | Frontier/FOV_hor_num: 15 12 | Frontier/FOV_ver_num: 10 13 | Frontier/cam_hor: 1.9 14 | Frontier/cam_ver: 1.046 15 | 16 | # Frontier/sensor_type: "Livox" 17 | # Frontier/FOV_hor_num: 35 18 | # Frontier/FOV_ver_num: 10 19 | # Frontier/livox_ver_up: 1.308 20 | # Frontier/livox_ver_low: -0.175 21 | 22 | Frontier/ray_samp_dist1: 0.2 23 | Frontier/ray_samp_dist2: 0.1 24 | Frontier/show_frontier: false 25 | Frontier/min_vp_num: 6 26 | 27 | block_map/RobotVisX: 0.81 28 | block_map/RobotVisY: 0.81 29 | block_map/RobotVisZ: 0.41 30 | block_map/swarm_tol: 0.3 31 | block_map/swarm_send_delay: 2.0 32 | block_map/swarm_pub_thresh: 0.975 33 | block_map/minX: -26.5 34 | block_map/minY: -26.5 35 | block_map/minZ: -0.1 36 | block_map/maxX: 26.5 37 | block_map/maxY: 26.5 38 | block_map/maxZ: 5.5 39 | block_map/blockX: 15 40 | block_map/blockY: 15 41 | block_map/blockZ: 15 42 | block_map/resolution: 0.1 43 | block_map/sensor_max_range: 5.0 44 | block_map/FFD: false 45 | block_map/depth: true 46 | block_map/update_freq: 10.0 47 | block_map/show_freq: 0.0 48 | block_map/depth_step: 2 49 | block_map/CamtoBody_Quater_x: -0.5 50 | block_map/CamtoBody_Quater_y: 0.5 51 | block_map/CamtoBody_Quater_z: -0.5 52 | block_map/CamtoBody_Quater_w: 0.5 53 | block_map/CamtoBody_x: 0.0 54 | block_map/CamtoBody_y: 0.0 55 | block_map/CamtoBody_z: 0.0 56 | block_map/occ_max: 0.90 57 | block_map/occ_min: 0.20 58 | block_map/pro_hit_occ: 0.90 59 | block_map/pro_miss_free: 0.75 60 | block_map/statistic_v: true 61 | block_map/show_block: false 62 | block_map/vis_mode: true 63 | block_map/min_finish_t: 45.0 64 | 65 | Exp/show_swarm_traj: false 66 | EXP/finish_hold_duration: 15.0 67 | Exp/robot_sizeX: 0.5 68 | Exp/robot_sizeY: 0.5 69 | Exp/robot_sizeZ: 0.5 70 | Exp/minX: -25.0 71 | Exp/minY: -25.0 72 | Exp/minZ: 0.0 73 | Exp/maxX: 25.0 74 | Exp/maxY: 25.0 75 | Exp/maxZ: 5.0 76 | Exp/traj_length: 10.0 77 | Exp/local_path_search: 1000 78 | Exp/global_path_search: 3000 79 | Exp/strong_check_interval: 3.0 80 | Exp/check_duration: 3.5 81 | Exp/exc_duration: 0.5 82 | Exp/replan_duration: 1.5 83 | Exp/takeoff_x: 0.0 84 | Exp/takeoff_y: 0.0 85 | Exp/takeoff_z: 1.0 86 | Exp/takeoff_yaw: 0.0 87 | Exp/reach_out_t: 0.1 88 | Exp/AutoTakeoff: true 89 | Exp/acc_off: -0.5 90 | Exp/statistic: true 91 | Exp/finish_thresh: 0.5 92 | Exp/swarm_check: true 93 | Exp/colli_range: 0.75 94 | #swarm 95 | # Exp/UAV_id: 1 96 | # Exp/drone_num: 1 97 | Exp/local_comm_freq: 10.0 98 | Exp/global_comm_freq: 0.5 99 | Exp/local_dist_thresh: 7.0 100 | 101 | GVD/show_gvd: false 102 | GVD/lambda: 0.1 103 | GVD/allowance: 0.08 104 | GVD/tau: 0.2 105 | GVD/partition_frequency: 0.33 106 | 107 | MR_DTG/Toporange: 10.0 108 | MR_DTG/H_thresh: 5.5 109 | MR_DTG/update_FF: false 110 | MR_DTG/resX: 2.0 111 | MR_DTG/resY: 2.0 112 | MR_DTG/resZ: 2.0 113 | MR_DTG/show_edge_details: false 114 | 115 | LowResMap/localgraph_sizex: 14.0 116 | LowResMap/localgraph_sizey: 14.0 117 | LowResMap/localgraph_sizez: 8.0 118 | LowResMap/node_x: 0.6 119 | LowResMap/node_y: 0.6 120 | LowResMap/node_z: 0.6 121 | LowResMap/blockX: 5 122 | LowResMap/blockY: 5 123 | LowResMap/blockZ: 3 124 | LowResMap/showmap: false 125 | LowResMap/debug: true 126 | LowResMap/lambda_heu: 5.0 127 | LowResMap/resolution: 0.1 128 | LowResMap/seg_length: 2.5 129 | LowResMap/prune_seg_length: 6.5 130 | LowResMap/occ_duration: 2.0 131 | LowResMap/unknown_duration: 0.2 132 | LowResMap/corridor_expX: 2 133 | LowResMap/corridor_expY: 2 134 | LowResMap/corridor_expZ: 1 135 | LowResMap/show_dtg: false 136 | 137 | opt/YawVel: 2.2 138 | opt/YawAcc: 2.2 139 | opt/MaxVel: 1.5 140 | opt/MaxAcc: 1.5 141 | opt/MaxJer: 25.0 142 | opt/SwarmAvoid: 1.2 143 | opt/WeiPos: 5000.0 144 | opt/WeiVel: 800.0 145 | opt/WeiAcc: 400.0 146 | opt/WeiJer: 50.0 147 | opt/WeiSwarm: 100.0 148 | opt/WeiminT: 10.0 149 | opt/WeiT: 200.0 150 | opt/smoothingEps: 0.01 151 | opt/integralIntervs: 16 152 | opt/RelCostTolL: 1.0e-5 153 | 154 | Computation/data_num: 3 155 | Computation/dir: /home/charliedog/data/swarm/testcity/MURDER 156 | Computation/data_name0: gvp 157 | Computation/data_name1: local 158 | Computation/data_name2: global 159 | 160 | Computation/vdata_num: 9 161 | Computation/vdata_name0: volume 162 | Computation/vdata_name1: DTG 163 | Computation/vdata_name2: pose 164 | Computation/vdata_name3: traj 165 | Computation/vdata_name4: job 166 | Computation/vdata_name5: state 167 | Computation/vdata_name6: map 168 | Computation/vdata_name7: mapreq 169 | Computation/vdata_name8: total -------------------------------------------------------------------------------- /Exploration/murder_swarm/resource/murder_testcity_ground.yaml: -------------------------------------------------------------------------------- 1 | Frontier/sample_max_range: 3.0 2 | Frontier/grid_scale: 2.0 3 | Frontier/viewpoint_thresh: 1.3 4 | Frontier/observe_thresh: 0.65 5 | Frontier/resample_duration: 0.8 6 | Frontier/sample_hor_dir_num: 10 7 | Frontier/sample_ver_dir_num: 3 8 | Frontier/sample_dist_num: 2 9 | Frontier/sensor_type: "Depth_Camera" 10 | Frontier/samp_free_thresh: 15 11 | Frontier/FOV_hor_num: 15 12 | Frontier/FOV_ver_num: 10 13 | # Frontier/sensor_type: "Livox" 14 | # Frontier/FOV_hor_num: 35 15 | # Frontier/FOV_ver_num: 10 16 | Frontier/cam_hor: 1.9 17 | Frontier/cam_ver: 1.046 18 | Frontier/livox_ver_low: -0.174 19 | Frontier/livox_ver_up: 1.308 20 | Frontier/ray_samp_dist1: 0.2 21 | Frontier/ray_samp_dist2: 0.1 22 | Frontier/show_frontier: true 23 | Frontier/min_vp_num: 6 24 | 25 | 26 | block_map/RobotVisX: 0.81 27 | block_map/RobotVisY: 0.81 28 | block_map/RobotVisZ: 0.41 29 | block_map/swarm_tol: 0.3 30 | block_map/swarm_send_delay: 2.0 31 | block_map/swarm_pub_thresh: 0.97 32 | block_map/minX: -26.5 33 | block_map/minY: -26.5 34 | block_map/minZ: -0.1 35 | block_map/maxX: 26.5 36 | block_map/maxY: 26.5 37 | block_map/maxZ: 5.5 38 | block_map/blockX: 15 39 | block_map/blockY: 15 40 | block_map/blockZ: 15 41 | block_map/resolution: 0.1 42 | block_map/sensor_max_range: 5.0 43 | block_map/FFD: false 44 | block_map/depth: true 45 | block_map/update_freq: 10.0 46 | block_map/show_freq: 10.0 47 | block_map/depth_step: 2 48 | block_map/CamtoBody_Quater_x: -0.5 49 | block_map/CamtoBody_Quater_y: 0.5 50 | block_map/CamtoBody_Quater_z: -0.5 51 | block_map/CamtoBody_Quater_w: 0.5 52 | block_map/CamtoBody_x: 0.0 53 | block_map/CamtoBody_y: 0.0 54 | block_map/CamtoBody_z: 0.0 55 | block_map/occ_max: 0.90 56 | block_map/occ_min: 0.20 57 | block_map/pro_hit_occ: 0.90 58 | block_map/pro_miss_free: 0.75 59 | block_map/statistic_v: true 60 | block_map/show_block: true 61 | block_map/vis_mode: true 62 | block_map/min_finish_t: 30.0 63 | 64 | Exp/show_swarm_traj: false 65 | EXP/finish_hold_duration: 15.0 66 | Exp/robot_sizeX: 0.48 67 | Exp/robot_sizeY: 0.48 68 | Exp/robot_sizeZ: 0.3 69 | Exp/minX: -25.0 70 | Exp/minY: -25.0 71 | Exp/minZ: 0.0 72 | Exp/maxX: 25.0 73 | Exp/maxY: 25.0 74 | Exp/maxZ: 5.0 75 | Exp/traj_length: 10.0 76 | Exp/local_path_search: 1000 77 | Exp/global_path_search: 3000 78 | Exp/strong_check_interval: 3.0 79 | Exp/check_duration: 3.5 80 | Exp/exc_duration: 0.5 81 | Exp/replan_duration: 1.5 82 | Exp/takeoff_x: 0.0 83 | Exp/takeoff_y: 0.0 84 | Exp/takeoff_z: 1.0 85 | Exp/takeoff_yaw: 0.0 86 | Exp/reach_out_t: 0.1 87 | Exp/AutoTakeoff: true 88 | Exp/statistic: true 89 | Exp/finish_thresh: 0.5 90 | 91 | #swarm 92 | # Exp/UAV_id: 1 93 | # Exp/drone_num: 1 94 | Exp/local_comm_freq: 5.0 95 | Exp/global_comm_freq: 0.5 96 | Exp/local_dist_thresh: 7.0 97 | 98 | GVD/show_gvd: true 99 | GVD/lambda: 0.1 100 | GVD/allowance: 0.2 101 | GVD/tau: 0.5 102 | GVD/partition_frequency: 1.0 103 | 104 | MR_DTG/Toporange: 10.0 105 | MR_DTG/H_thresh: 5.5 106 | MR_DTG/update_FF: false 107 | MR_DTG/resX: 2.0 108 | MR_DTG/resY: 2.0 109 | MR_DTG/resZ: 2.0 110 | MR_DTG/show_edge_details: true 111 | 112 | LowResMap/localgraph_sizex: 14.0 113 | LowResMap/localgraph_sizey: 14.0 114 | LowResMap/localgraph_sizez: 8.0 115 | LowResMap/node_x: 0.6 116 | LowResMap/node_y: 0.6 117 | LowResMap/node_z: 0.6 118 | LowResMap/blockX: 5 119 | LowResMap/blockY: 5 120 | LowResMap/blockZ: 3 121 | LowResMap/showmap: true 122 | LowResMap/debug: true 123 | LowResMap/lambda_heu: 5.0 124 | LowResMap/resolution: 0.1 125 | LowResMap/seg_length: 2.5 126 | LowResMap/prune_seg_length: 6.5 127 | LowResMap/occ_duration: 2.0 128 | LowResMap/unknown_duration: 0.2 129 | LowResMap/corridor_expX: 2 130 | LowResMap/corridor_expY: 2 131 | LowResMap/corridor_expZ: 1 132 | LowResMap/show_dtg: false 133 | 134 | opt/YawVel: 1.8 135 | opt/YawAcc: 1.8 136 | opt/MaxVel: 1.5 137 | opt/MaxAcc: 1.5 138 | opt/MaxJer: 15.0 139 | opt/SwarmAvoid: 0.9 140 | opt/WeiPos: 5000.0 141 | opt/WeiVel: 500.0 142 | opt/WeiAcc: 100.0 143 | opt/WeiJer: 50.0 144 | opt/WeiSwarm: 500.0 145 | opt/WeiminT: 10.0 146 | opt/WeiT: 200.0 147 | opt/smoothingEps: 0.01 148 | opt/integralIntervs: 16 149 | opt/RelCostTolL: 1.0e-5 150 | 151 | Computation/data_num: 3 152 | Computation/dir: /home/charliedog/data/swarm/testcity/MURDER 153 | Computation/data_name0: gvp 154 | Computation/data_name1: local 155 | Computation/data_name2: global 156 | 157 | Computation/vdata_num: 9 158 | Computation/vdata_name0: volume 159 | Computation/vdata_name1: DTG 160 | Computation/vdata_name2: pose 161 | Computation/vdata_name3: traj 162 | Computation/vdata_name4: job 163 | Computation/vdata_name5: state 164 | Computation/vdata_name6: map 165 | Computation/vdata_name7: mapreq 166 | Computation/vdata_name8: total -------------------------------------------------------------------------------- /Exploration/murder_swarm/src/ground.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void Ground::init(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private){ 4 | std::string ns = ros::this_node::getName(); 5 | // nh_private_.param(ns + "/Exp/traj_length", traj_length_, 10.0); 6 | nh_ = nh; 7 | nh_private_ = nh_private; 8 | SDM_.init(nh_, nh_private_); 9 | 10 | BM_.SetSwarmDataManager(&SDM_); 11 | BM_.init(nh_, nh_private_); 12 | CM_.init(nh_, nh_private_); 13 | LRM_.SetColorManager(&CM_); 14 | 15 | 16 | LRM_.SetMap(&BM_); 17 | LRM_.init(nh, nh_private); 18 | TrajOpt_.Init(nh_, nh_private_); 19 | 20 | FG_.SetMap(BM_); 21 | FG_.SetLowresMap(LRM_); 22 | FG_.SetColorManager(CM_); 23 | FG_.SetSwarmDataManager(&SDM_); 24 | FG_.init(nh_, nh_private_); 25 | 26 | MDTG_.SetFrontierMap(&FG_); 27 | MDTG_.SetColorManager(&CM_); 28 | MDTG_.SetBlockMap(&BM_); 29 | MDTG_.SetLowresMap(&LRM_); 30 | MDTG_.SetSwarmDataManager(&SDM_); 31 | MDTG_.local_fn_ = &local_fn_; 32 | MDTG_.local_hn_ = &local_hn_; 33 | MDTG_.GVP_hn_ = &GVP_hn_; 34 | MDTG_.init(nh_, nh_private_); 35 | 36 | GVP_.SetLowresMap(&LRM_); 37 | GVP_.SetDTGMap(&MDTG_); 38 | GVP_.SetFrontierMap(&FG_); 39 | GVP_.SetColorManager(&CM_); 40 | GVP_.SetSwarmDataManager(&SDM_); 41 | GVP_.local_fn_ = &local_fn_; 42 | GVP_.local_hn_ = &local_hn_; 43 | GVP_.GVP_hn_ = &GVP_hn_; 44 | GVP_.init(nh_, nh_private_); 45 | 46 | CreateVisModels(); 47 | 48 | traj_vis_pub_ = nh_.advertise(ns + "/Trajs", 5); 49 | pose_vis_pub_ = nh_.advertise(ns + "/Poses", 5); 50 | traj_timer_ = nh_.createTimer(ros::Duration(0.05), &Ground::TrajVisCallback, this); 51 | pose_timer_ = nh_.createTimer(ros::Duration(0.05), &Ground::PoseVisCallback, this); 52 | } 53 | 54 | void Ground::CreateVisModels(){ 55 | vis_models_.markers.resize(5 * SDM_.drone_num_); 56 | poses_.resize(SDM_.drone_num_); 57 | for(int i = 0; i < SDM_.drone_num_; i++){ 58 | vis_models_.markers[i*5 + 0].header.frame_id = "world"; 59 | vis_models_.markers[i*5 + 0].header.stamp = ros::Time::now(); 60 | vis_models_.markers[i*5 + 0].id = i*5 + 0; 61 | vis_models_.markers[i*5 + 0].action = visualization_msgs::Marker::ADD; 62 | vis_models_.markers[i*5 + 0].type = visualization_msgs::Marker::SPHERE; 63 | vis_models_.markers[i*5 + 0].scale.x = 0.30; 64 | vis_models_.markers[i*5 + 0].scale.y = 0.30; 65 | vis_models_.markers[i*5 + 0].scale.z = 0.04; 66 | vis_models_.markers[i*5 + 0].color = CM_.Id2Color(i+1, 1.0); 67 | poses_[i].orientation.w = 1.0; 68 | poses_[i].orientation.x = 0.0; 69 | poses_[i].orientation.y = 0.0; 70 | poses_[i].orientation.z = 0.0; 71 | poses_[i].position.x = 0.0; 72 | poses_[i].position.y = 0.0; 73 | poses_[i].position.z = 0.0; 74 | 75 | vis_models_.markers[i*5 + 1] = vis_models_.markers[i*5 + 0]; 76 | vis_models_.markers[i*5 + 1].id = i*5 + 1; 77 | vis_models_.markers[i*5 + 2] = vis_models_.markers[i*5 + 0]; 78 | vis_models_.markers[i*5 + 2].id = i*5 + 2; 79 | vis_models_.markers[i*5 + 3] = vis_models_.markers[i*5 + 0]; 80 | vis_models_.markers[i*5 + 3].id = i*5 + 3; 81 | 82 | vis_models_.markers[i*5 + 4] = vis_models_.markers[i*5 + 0]; 83 | vis_models_.markers[i*5 + 4].id = i*5 + 4; 84 | vis_models_.markers[i*5 + 4].type = visualization_msgs::Marker::LINE_LIST; 85 | vis_models_.markers[i*5 + 4].scale.x = 0.025; 86 | vis_models_.markers[i*5 + 4].scale.y = 0.025; 87 | vis_models_.markers[i*5 + 4].scale.z = 0.025; 88 | 89 | geometry_msgs::Point pt; 90 | 91 | pt.x = 0.225; 92 | pt.y = 0.225; 93 | pt.z = -0.02; 94 | vis_models_.markers[i*5 + 4].points.emplace_back(pt); 95 | pt.y = -0.225; 96 | pt.x = -0.225; 97 | vis_models_.markers[i*5 + 4].points.emplace_back(pt); 98 | pt.x = -0.225; 99 | pt.y = 0.225; 100 | vis_models_.markers[i*5 + 4].points.emplace_back(pt); 101 | pt.x = 0.225; 102 | pt.y = -0.225; 103 | vis_models_.markers[i*5 + 4].points.emplace_back(pt); 104 | } 105 | } 106 | 107 | void Ground::TrajVisCallback(const ros::TimerEvent &e){ 108 | visualization_msgs::MarkerArray mka; 109 | mka.markers.resize(SDM_.drone_num_); 110 | mka.markers[0].header.frame_id = "world"; 111 | mka.markers[0].header.stamp = ros::Time::now(); 112 | mka.markers[0].action = visualization_msgs::Marker::ADD; 113 | mka.markers[0].type = visualization_msgs::Marker::SPHERE_LIST; 114 | mka.markers[0].scale.x = 0.1; 115 | mka.markers[0].scale.y = 0.1; 116 | mka.markers[0].scale.z = 0.1; 117 | mka.markers[0].pose.orientation.w = 1; 118 | mka.markers[0].pose.orientation.x = 0; 119 | mka.markers[0].pose.orientation.y = 0; 120 | mka.markers[0].pose.orientation.z = 0; 121 | mka.markers[0].color.a = 0.7; 122 | mka.markers[0].color.r = 0.3; 123 | mka.markers[0].color.g = 0.9; 124 | mka.markers[0].color.b = 0.2; 125 | double cur_t = ros::WallTime::now().toSec(); 126 | geometry_msgs::Point pt; 127 | for(int i = 0; i < SDM_.drone_num_; i++){ 128 | mka.markers[i] = mka.markers[0]; 129 | mka.markers[i].id = i; 130 | double total_t = SDM_.trajs_[i].getTotalDuration(); 131 | if(cur_t - SDM_.start_t_[i] < total_t) { 132 | for(double dt = 0; dt < total_t; dt += 0.05){ 133 | Eigen::Vector3d p; 134 | p = SDM_.trajs_[i].getPos(dt); 135 | pt.x = p(0); 136 | pt.y = p(1); 137 | pt.z = p(2); 138 | mka.markers[i].points.emplace_back(pt); 139 | } 140 | } 141 | if(mka.markers[i].points.empty()) { 142 | mka.markers[i].action = visualization_msgs::Marker::DELETE; 143 | } 144 | } 145 | traj_vis_pub_.publish(mka); 146 | } 147 | 148 | void Ground::PoseVisCallback(const ros::TimerEvent &e){ 149 | LoadVisModels(); 150 | pose_vis_pub_.publish(vis_models_); 151 | } 152 | -------------------------------------------------------------------------------- /Exploration/murder_swarm/src/ground_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv){ 4 | ros::init(argc, argv, "ground_node"); 5 | ros::NodeHandle nh, nh_private("~"); 6 | 7 | Ground GD; 8 | GD.init(nh, nh_private); 9 | 10 | ros::spin(); 11 | return 0; 12 | } -------------------------------------------------------------------------------- /Exploration/murder_swarm/src/murder_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | int main(int argc, char** argv){ 9 | ros::init(argc, argv, "murder_node"); 10 | ros::NodeHandle nh, nh_private("~"); 11 | 12 | google::InitGoogleLogging(argv[0]); 13 | google::ParseCommandLineFlags(&argc, &argv, true); 14 | google::InstallFailureSignalHandler(); 15 | 16 | MurderFSM M_FSM; 17 | M_FSM.init(nh, nh_private); 18 | ros::spin(); 19 | return 0; 20 | } -------------------------------------------------------------------------------- /Exploration/murder_swarm/src/swarm_trigger.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | int main(int argc, char** argv){ 8 | ros::init(argc, argv, "trigger"); 9 | ros::NodeHandle nh, nh_private("~"); 10 | 11 | ros::Publisher trigger_pub; 12 | 13 | trigger_pub = nh.advertise("/takeoff", 10); 14 | 15 | ros::Duration(0.5).sleep(); 16 | std_msgs::Empty msg; 17 | trigger_pub.publish(msg); 18 | ROS_WARN("go go go!!!"); 19 | ros::Duration(0.5).sleep(); 20 | return 0; 21 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 NKU-RaHAIC 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /MSGS/exp_comm_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(exp_comm_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | nav_msgs 8 | sensor_msgs 9 | geometry_msgs 10 | swarm_exp_msgs 11 | message_generation 12 | ) 13 | 14 | include_directories( 15 | include 16 | ${catkin_INCLUDE_DIRS} 17 | ) 18 | 19 | ## Generate messages in the 'msg' folder 20 | add_message_files( 21 | FILES 22 | DtgBagAnswer.msg 23 | DtgBagC.msg 24 | SwarmTrajC.msg 25 | IdPoseC.msg 26 | SwarmJobC.msg 27 | SwarmStateC.msg 28 | MapC.msg 29 | MapReqC.msg 30 | ) 31 | 32 | generate_messages( 33 | DEPENDENCIES 34 | std_msgs 35 | nav_msgs 36 | sensor_msgs 37 | geometry_msgs 38 | swarm_exp_msgs 39 | ) 40 | 41 | catkin_package( 42 | CATKIN_DEPENDS message_runtime 43 | ) 44 | 45 | -------------------------------------------------------------------------------- /MSGS/exp_comm_msgs/msg/DtgBagAnswer.msg: -------------------------------------------------------------------------------- 1 | uint8 id 2 | uint8 to_uav 3 | uint32 bag_id -------------------------------------------------------------------------------- /MSGS/exp_comm_msgs/msg/DtgBagC.msg: -------------------------------------------------------------------------------- 1 | float64 pub_t 2 | uint32 id 3 | uint8 from_uav 4 | swarm_exp_msgs/DtgFFEdge[] FFedges 5 | swarm_exp_msgs/DtgHFEdge[] HFedges 6 | swarm_exp_msgs/DtgHHEdge[] HHedges 7 | swarm_exp_msgs/DtgHNode[] Hnodes 8 | swarm_exp_msgs/DtgFNode[] Fnodes -------------------------------------------------------------------------------- /MSGS/exp_comm_msgs/msg/IdPoseC.msg: -------------------------------------------------------------------------------- 1 | uint8 id 2 | float64 pub_t 3 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /MSGS/exp_comm_msgs/msg/MapC.msg: -------------------------------------------------------------------------------- 1 | uint16 f_id 2 | uint8 block_id #1-8 3 | uint8 block_state #0: unknown 1:occ 2:free 3:mix 4 | uint8[] flags #00 00 00 00 (free occ)(free occ)(free occ)(free occ) -------------------------------------------------------------------------------- /MSGS/exp_comm_msgs/msg/MapReqC.msg: -------------------------------------------------------------------------------- 1 | uint16[] f_id 2 | uint8[] block_id 3 | uint8 flag -------------------------------------------------------------------------------- /MSGS/exp_comm_msgs/msg/SwarmJobC.msg: -------------------------------------------------------------------------------- 1 | float64 pub_t 2 | uint8 from_uav 3 | uint8 JobState #0000 0(no job = 0/ new job = 1)(finish = 1)(local = 0/ global = 1) 4 | int32 target_fn 5 | int32 target_hn 6 | float32 dist_to_fn 7 | float32 dist_to_hn 8 | 9 | 10 | -------------------------------------------------------------------------------- /MSGS/exp_comm_msgs/msg/SwarmStateC.msg: -------------------------------------------------------------------------------- 1 | float64 pub_t 2 | uint8 from_uav 3 | uint8 flags #0000 000(quick communication) 4 | uint16[] local_fn 5 | float32[] dist_to_local_fn 6 | 7 | uint16[] connect_fn 8 | float32[] dist_to_connect_fn 9 | 10 | uint32[] connect_hn 11 | uint32[] hn_pos_idx 12 | float32[] dist_to_connect_hn 13 | 14 | -------------------------------------------------------------------------------- /MSGS/exp_comm_msgs/msg/SwarmTrajC.msg: -------------------------------------------------------------------------------- 1 | float64 pub_t 2 | 3 | uint8 id 4 | 5 | float64 start_t 6 | int8 order_p 7 | float32[] t_p 8 | geometry_msgs/Point[] coef_p 9 | 10 | # int8 order_yaw 11 | # float32[] t_yaw 12 | # float32[] coef_yaw -------------------------------------------------------------------------------- /MSGS/exp_comm_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | exp_comm_msgs 4 | 0.0.0 5 | The exp_comm_msgs package 6 | 7 | 8 | 9 | CharlieDog 10 | 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | catkin 51 | roscpp 52 | std_msgs 53 | sensor_msgs 54 | nav_msgs 55 | geometry_msgs 56 | swarm_exp_msgs 57 | message_generation 58 | 59 | roscpp 60 | std_msgs 61 | sensor_msgs 62 | nav_msgs 63 | geometry_msgs 64 | swarm_exp_msgs 65 | message_runtime 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(swarm_exp_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | nav_msgs 8 | sensor_msgs 9 | geometry_msgs 10 | message_generation 11 | ) 12 | 13 | include_directories( 14 | include 15 | ${catkin_INCLUDE_DIRS} 16 | ) 17 | 18 | ## Generate messages in the 'msg' folder 19 | add_message_files( 20 | FILES 21 | SwarmTraj.msg 22 | LocalTraj.msg 23 | DtgFFEdge.msg 24 | DtgHFEdge.msg 25 | DtgHHEdge.msg 26 | DtgHNode.msg 27 | DtgFNode.msg 28 | DtgBag.msg 29 | IdOdom.msg 30 | IdPose.msg 31 | ) 32 | 33 | generate_messages( 34 | DEPENDENCIES 35 | std_msgs 36 | nav_msgs 37 | sensor_msgs 38 | geometry_msgs 39 | ) 40 | 41 | catkin_package( 42 | CATKIN_DEPENDS message_runtime 43 | ) 44 | 45 | -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/msg/DtgBag.msg: -------------------------------------------------------------------------------- 1 | uint8[] to_uavs 2 | uint32 id 3 | uint8 from_uav 4 | 5 | swarm_exp_msgs/DtgHFEdge[] HFedges 6 | swarm_exp_msgs/DtgHHEdge[] HHedges 7 | swarm_exp_msgs/DtgHNode[] Hnodes 8 | swarm_exp_msgs/DtgFNode[] Fnodes -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/msg/DtgFFEdge.msg: -------------------------------------------------------------------------------- 1 | uint16 head_f_id 2 | uint16 tail_f_id 3 | uint32[] points_idx #low res map 4 | 5 | uint8 head_v_id #viewpoint id of head fn 6 | uint8 tail_v_id #viewpoint id of tail fn 7 | uint8 ffe_id #who creates 8 | -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/msg/DtgFNode.msg: -------------------------------------------------------------------------------- 1 | uint16 f_id 2 | bool alive 3 | uint8[] vp_flags #(alive dead)(alive dead)(alive dead)(alive dead) 4 | uint8 need_help # ask other uav for edge 5 | -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/msg/DtgHFEdge.msg: -------------------------------------------------------------------------------- 1 | uint32[] points_idx #low res map 2 | float64 pub_t 3 | uint16 f_id 4 | uint32 h_id 5 | uint8 vp_id 6 | #uint8 hfe_id #who creates 7 | -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/msg/DtgHHEdge.msg: -------------------------------------------------------------------------------- 1 | #uint8 hhe_id #who creates 2 | #bool erase 3 | float64 pub_t 4 | uint32[] points_idx #low res map 5 | uint32 head_h_id 6 | uint32 tail_h_id -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/msg/DtgHNode.msg: -------------------------------------------------------------------------------- 1 | uint32 h_id 2 | uint32 pos_idx #low res map -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/msg/IdOdom.msg: -------------------------------------------------------------------------------- 1 | uint8 id 2 | nav_msgs/Odometry odoms -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/msg/IdPose.msg: -------------------------------------------------------------------------------- 1 | uint8[] to_uavs 2 | 3 | uint8 id 4 | float64 pub_t 5 | geometry_msgs/Pose pose -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/msg/LocalTraj.msg: -------------------------------------------------------------------------------- 1 | uint8 from_uav 2 | uint8[] to_uavs 3 | 4 | int8 state 5 | 6 | geometry_msgs/Point recover_pt 7 | 8 | float64 start_t 9 | int8 order_p 10 | float32[] t_p 11 | geometry_msgs/Point[] coef_p 12 | 13 | int8 order_yaw 14 | float32[] t_yaw 15 | float32[] coef_yaw 16 | -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/msg/SwarmTraj.msg: -------------------------------------------------------------------------------- 1 | uint8[] to_uavs 2 | uint8 from_uav 3 | 4 | float64 start_t 5 | int8 order_p 6 | float32[] t_p 7 | geometry_msgs/Point[] coef_p 8 | 9 | -------------------------------------------------------------------------------- /MSGS/swarm_exp_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | swarm_exp_msgs 4 | 0.0.0 5 | The swarm_exp_msgs package 6 | 7 | 8 | 9 | CharlieDog 10 | 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | catkin 51 | roscpp 52 | std_msgs 53 | sensor_msgs 54 | nav_msgs 55 | geometry_msgs 56 | message_generation 57 | 58 | roscpp 59 | std_msgs 60 | sensor_msgs 61 | nav_msgs 62 | geometry_msgs 63 | message_runtime 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /Mapping/block_map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(block_map) 3 | 4 | add_definitions(-std=c++14) 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(catkin_simple REQUIRED) 8 | catkin_simple(ALL_DEPS_REQUIRED) 9 | 10 | include_directories( 11 | ${catkin_INCLUDE_DIRS} 12 | ) 13 | 14 | cs_add_library(${PROJECT_NAME}_raycast src/raycast.cpp) 15 | cs_add_library(${PROJECT_NAME}_grid src/block_map.cpp) 16 | cs_add_library(${PROJECT_NAME}_color src/color_manager.cpp) 17 | 18 | target_link_libraries(${PROJECT_NAME}_grid ${PROJECT_NAME}_raycast) 19 | ########## 20 | # EXPORT # 21 | ########## 22 | cs_install() 23 | cs_export() 24 | 25 | -------------------------------------------------------------------------------- /Mapping/block_map/color/Abbey_Road.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/Abbey_Road.png -------------------------------------------------------------------------------- /Mapping/block_map/color/Abbey_Road.yaml: -------------------------------------------------------------------------------- 1 | block_map/HeightcolorR: [44, 79, 83, 239] 2 | block_map/HeightcolorG: [42, 97, 150, 238] 3 | block_map/HeightcolorB: [51, 106, 194, 224] 4 | -------------------------------------------------------------------------------- /Mapping/block_map/color/Abbey_road.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/Abbey_road.jpg -------------------------------------------------------------------------------- /Mapping/block_map/color/Burn.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/Burn.jpg -------------------------------------------------------------------------------- /Mapping/block_map/color/Burn.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/Burn.png -------------------------------------------------------------------------------- /Mapping/block_map/color/Burn.yaml: -------------------------------------------------------------------------------- 1 | block_map/HeightcolorR: [33, 193, 228, 232, 254, 174] 2 | block_map/HeightcolorG: [18, 0, 41, 114, 244, 27] 3 | block_map/HeightcolorB: [21, 113, 41, 54, 232, 24] 4 | -------------------------------------------------------------------------------- /Mapping/block_map/color/DrFeelgood.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/DrFeelgood.jpg -------------------------------------------------------------------------------- /Mapping/block_map/color/DrFeelgood.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/DrFeelgood.png -------------------------------------------------------------------------------- /Mapping/block_map/color/DrFeelgood.yaml: -------------------------------------------------------------------------------- 1 | block_map/HeightcolorR: [77, 117, 123, 135, 153, 213] 2 | block_map/HeightcolorG: [76, 110, 137, 160, 187, 191] 3 | block_map/HeightcolorB: [54, 65, 75, 123, 152, 92] 4 | -------------------------------------------------------------------------------- /Mapping/block_map/color/Odessey_Oracle.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/Odessey_Oracle.jpg -------------------------------------------------------------------------------- /Mapping/block_map/color/Odessey_Oracle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/Odessey_Oracle.png -------------------------------------------------------------------------------- /Mapping/block_map/color/Odessey_Oracle.yaml: -------------------------------------------------------------------------------- 1 | block_map/HeightcolorR: [212, 234, 138, 188, 199, 222] 2 | block_map/HeightcolorG: [167, 69, 131, 212, 219, 171] 3 | block_map/HeightcolorB: [186, 86, 156, 52, 117, 84] 4 | -------------------------------------------------------------------------------- /Mapping/block_map/color/Paranoid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/Paranoid.png -------------------------------------------------------------------------------- /Mapping/block_map/color/Paranoid.yaml: -------------------------------------------------------------------------------- 1 | block_map/HeightcolorR: [44, 71, 198, 234, 242, 180, 40] 2 | block_map/HeightcolorG: [21, 80, 66, 114, 177, 204, 144] 3 | block_map/HeightcolorB: [48, 147, 92, 130, 144, 232, 207] 4 | -------------------------------------------------------------------------------- /Mapping/block_map/color/Powerslave.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/Powerslave.png -------------------------------------------------------------------------------- /Mapping/block_map/color/Powerslave.yaml: -------------------------------------------------------------------------------- 1 | block_map/HeightcolorR: [241, 232, 234, 218, 230, 74] 2 | block_map/HeightcolorG: [214, 184, 172, 140, 194, 110] 3 | block_map/HeightcolorB: [180, 142, 127, 83, 89, 140] 4 | -------------------------------------------------------------------------------- /Mapping/block_map/color/TDSOTM.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/TDSOTM.jpg -------------------------------------------------------------------------------- /Mapping/block_map/color/TDSOTM.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/TDSOTM.png -------------------------------------------------------------------------------- /Mapping/block_map/color/TDSOTM.yaml: -------------------------------------------------------------------------------- 1 | block_map/HeightcolorR: [250, 253, 220, 149, 35, 181] 2 | block_map/HeightcolorG: [44, 236, 255, 253, 191, 121] 3 | block_map/HeightcolorB: [57, 60, 63, 60, 237, 243] 4 | -------------------------------------------------------------------------------- /Mapping/block_map/color/ViewpointColor.yaml: -------------------------------------------------------------------------------- 1 | Vp/colorR: [150] 2 | Vp/colorG: [150] 3 | Vp/colorB: [150] -------------------------------------------------------------------------------- /Mapping/block_map/color/color_id.yaml: -------------------------------------------------------------------------------- 1 | block_map/colorR: [150, 0, 255, 124, 241, 0, 255, 255, 0, 243, 143, 112] 2 | block_map/colorG: [150, 229, 165, 252, 84, 0, 0, 44, 212, 110, 255, 0] 3 | block_map/colorB: [150, 238, 0, 0, 76, 255, 255, 44, 112, 50, 0, 255] -------------------------------------------------------------------------------- /Mapping/block_map/color/paranoid.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/paranoid.jpg -------------------------------------------------------------------------------- /Mapping/block_map/color/powerslave.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/Mapping/block_map/color/powerslave.jpg -------------------------------------------------------------------------------- /Mapping/block_map/include/block_map/color_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef COLOR_MANAGER_H_ 2 | #define COLOR_MANAGER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | 13 | class ColorManager{ 14 | public: 15 | ColorManager(){}; 16 | ~ColorManager(){}; 17 | void init(ros::NodeHandle &nh, ros::NodeHandle &nh_private); 18 | inline std_msgs::ColorRGBA Id2Color(int idx, double a); 19 | 20 | vector c_l_; 21 | }; 22 | 23 | 24 | inline std_msgs::ColorRGBA ColorManager::Id2Color(int idx, double a){ 25 | std_msgs::ColorRGBA color; 26 | idx = idx % int(c_l_.size()); 27 | color = c_l_[idx]; 28 | color.a = a; 29 | return color; 30 | } 31 | 32 | #endif -------------------------------------------------------------------------------- /Mapping/block_map/include/block_map/mapping_struct.h: -------------------------------------------------------------------------------- 1 | #ifndef MAPPING_STRUCT_H_ 2 | #define MAPPING_STRUCT_H_ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | using namespace std; 16 | using namespace Eigen; 17 | 18 | namespace BlockMapStruct{ 19 | 20 | enum GBSTATE{ 21 | UNKNOWN, 22 | MIXED, 23 | OCCUPIED, 24 | FREE 25 | }; 26 | 27 | enum VoxelState{ 28 | unknown, 29 | free, 30 | occupied, 31 | out 32 | }; 33 | 34 | struct SwarmBlock{ 35 | uint16_t id_; 36 | Eigen::Vector3d up_, down_; 37 | vector exploration_rate_; 38 | vector last_pub_rate_; 39 | vector to_pub_; 40 | uint8_t sub_num_; 41 | }; 42 | 43 | struct Grid_Block{ 44 | Grid_Block() {state_ = UNKNOWN;}; 45 | ~Grid_Block() {}; 46 | void Awake(float occ, float free){ //if state == UNKNOWN/OCCUPIED/FREE, init odds_log_ of this block 47 | if(state_ == UNKNOWN){ 48 | odds_log_.resize(block_size_.x() * block_size_.y() * block_size_.z(), free - 999.0); 49 | flags_.resize(odds_log_.size(), 0); 50 | } 51 | else if(state_ == OCCUPIED){ 52 | odds_log_.resize(block_size_.x() * block_size_.y() * block_size_.z(), occ); 53 | flags_.resize(odds_log_.size(), 0); 54 | } 55 | else if(state_ == FREE){ 56 | odds_log_.resize(block_size_.x() * block_size_.y() * block_size_.z(), free); 57 | flags_.resize(odds_log_.size(), 0); 58 | } 59 | state_ = MIXED; 60 | } 61 | Vector3i origin_; 62 | Vector3i block_size_; 63 | unsigned state_; 64 | vector odds_log_; 65 | vector flags_; //0000 0_(need to be show)_(is ray end occupied flag)_(casted) 66 | bool show_; 67 | int free_num_, occ_num_, unk_num_; 68 | int free_max_num_, occ_max_num_; 69 | }; 70 | 71 | struct FFD_Grid{ 72 | double far_depth_; 73 | double close_depth_; 74 | double max_depth_; 75 | 76 | double dist2depth_; 77 | bool is_frontier_; 78 | bool new_iter_; 79 | }; 80 | } 81 | 82 | #endif -------------------------------------------------------------------------------- /Mapping/block_map/include/block_map/raycast.h: -------------------------------------------------------------------------------- 1 | #ifndef BLOCKMAP_RAYCAST_H_ 2 | #define BLOCKMAP_RAYCAST_H_ 3 | 4 | #include 5 | #include 6 | 7 | double signum(double x); 8 | 9 | double mod(double value, double modulus); 10 | 11 | double intbound(double s, double ds); 12 | 13 | void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, 14 | const Eigen::Vector3d& max, int& output_points_cnt, Eigen::Vector3d* output); 15 | 16 | void Raycast(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Vector3d& min, 17 | const Eigen::Vector3d& max, std::vector* output); 18 | 19 | class RayCaster { 20 | private: 21 | /* data */ 22 | Eigen::Vector3d start_; 23 | Eigen::Vector3d end_; 24 | Eigen::Vector3d direction_; 25 | Eigen::Vector3d min_; 26 | Eigen::Vector3d max_; 27 | int x_; 28 | int y_; 29 | int z_; 30 | int endX_; 31 | int endY_; 32 | int endZ_; 33 | double maxDist_; 34 | double dx_; 35 | double dy_; 36 | double dz_; 37 | int stepX_; 38 | int stepY_; 39 | int stepZ_; 40 | double tMaxX_; 41 | double tMaxY_; 42 | double tMaxZ_; 43 | double tDeltaX_; 44 | double tDeltaY_; 45 | double tDeltaZ_; 46 | double dist_; 47 | 48 | int step_num_; 49 | 50 | public: 51 | RayCaster(/* args */) { 52 | } 53 | ~RayCaster() { 54 | } 55 | 56 | bool setInput(const Eigen::Vector3d& start, 57 | const Eigen::Vector3d& end /* , const Eigen::Vector3d& min, 58 | const Eigen::Vector3d& max */); 59 | 60 | bool step(Eigen::Vector3d& ray_pt); 61 | }; 62 | 63 | #endif // RAYCAST_H_ -------------------------------------------------------------------------------- /Mapping/block_map/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | block_map 4 | 0.0.0 5 | The block_map package 6 | 7 | 8 | 9 | 10 | CharlieDog 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | catkin 19 | catkin_simple 20 | 21 | roscpp 22 | tf 23 | visualization_msgs 24 | pcl_conversions 25 | pcl_ros 26 | 27 | cv_bridge 28 | 29 | sensor_msgs 30 | nav_msgs 31 | geometry_msgs 32 | swarm_data 33 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /Mapping/block_map/src/color_manager.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void ColorManager::init(ros::NodeHandle &nh, ros::NodeHandle &nh_private){ 4 | vector CR, CB, CG; 5 | std::string ns = ros::this_node::getName(); 6 | 7 | nh_private.param(ns + "/block_map/colorR", 8 | CR, {}); 9 | nh_private.param(ns + "/block_map/colorB", 10 | CG, {}); 11 | nh_private.param(ns + "/block_map/colorG", 12 | CB, {}); 13 | 14 | std_msgs::ColorRGBA color; 15 | for(int i = 0; i < CG.size(); i++){ 16 | color.a = 1.0; 17 | cout< 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | using namespace std; 16 | using namespace Eigen; 17 | 18 | namespace FrontierGridStruct{ 19 | enum SensorType{LIVOX, CAMERA}; 20 | 21 | struct CoarseFrontier{ 22 | uint8_t f_state_; //0: unexplored; 1:exploring; 2: explored; 23 | uint8_t owner_; 24 | float owner_dist_; 25 | 26 | // vector dirs_state_; //0: unexplored; 1:exploring; 2: explored; 27 | vector dirs_free_num_; 28 | Eigen::Vector3d up_, down_, center_; 29 | 30 | vector local_vps_; //0: unsampled; 1:alive; 2: dead; 31 | // vector public_vps_; //0: unsampled; 1:alive; 2: dead; 32 | double last_sample_; 33 | double last_strong_check_; 34 | int unknown_num_, thresh_num_; 35 | bitset<4> flags_; //(local new)(show)(sample)(temp) 36 | }; 37 | } 38 | 39 | #endif -------------------------------------------------------------------------------- /Mapping/frontier_grid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | frontier_grid 4 | 0.0.0 5 | The frontier_grid package 6 | 7 | 8 | 9 | 10 | CharlieDog 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | catkin 19 | catkin_simple 20 | 21 | roscpp 22 | tf 23 | visualization_msgs 24 | pcl_conversions 25 | pcl_ros 26 | 27 | block_map 28 | lowres_map 29 | cv_bridge 30 | 31 | sensor_msgs 32 | nav_msgs 33 | geometry_msgs 34 | swarm_data 35 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /Mapping/lowres_map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(lowres_map) 3 | 4 | add_definitions(-std=c++14) 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(catkin_simple REQUIRED) 8 | catkin_simple(ALL_DEPS_REQUIRED) 9 | 10 | include_directories( 11 | ${catkin_INCLUDE_DIRS} 12 | ) 13 | 14 | cs_add_library(${PROJECT_NAME} src/lowres_map.cpp src/path_search.cpp) 15 | ########## 16 | # EXPORT # 17 | ########## 18 | cs_install() 19 | cs_export() -------------------------------------------------------------------------------- /Mapping/lowres_map/Readme.md: -------------------------------------------------------------------------------- 1 | # LowResolutionMap Update 2 | 3 | ## Brief: 4 | LRM is composed of a set of lowresolution nodes, which are updated according to the precise map. 5 | 6 | ## Input: 7 | robot pos, current observed point cloud. 8 | 9 | ## Process Results: 10 | Get the local traversable(TS) and untraversable nodes(NTS). The NTS nodes will be marked as *Xnode*, which will not be updated in few seconds. 11 | If updated **normally**, the interconnected nodes inside a bounding box will be updated. 12 | If updated **topologically**, nodes to robot within a certain L1 dist or inside the current FOV will be updated using the Djkstra algorithm. Whats more, the Djkstra paths can be record for the DTG maintenance. 13 | 14 | ## Process(Topo): 15 | ``` 16 | 1. Set Xnodes according to pointcloud. 17 | 2. Set current FOV. 18 | 3. Clear last local nodes. 19 | # Expand Topological map 20 | 3. Push the node of current robot pos into TS proirity set or NTS priority set. 21 | 4. Two Djkstra(TS and NTS) search are processed in turn: 22 | -------------------------------------------------------#NTS Djkstra 23 | 1. Pop front node in the NTS priority set and check the status of the node. 24 | 2. If the status is "occupied" or "unknown"(may be set fronmtier in the future), it will be set as "Xnode". 25 | 3. If the status is "free", it will be set as "local" and push its neighbours into TS proirity set or NTS priority set. If the node is connected by some hnodes, those hnodes are recorded. 26 | ------------------------------------------------------- 27 | -------------------------------------------------------#TS Djkstra 28 | 1. Pop front node in the TS priority set. 29 | 2. The node will be set as "local" and push its neighbours into TS proirity set or NTS priority set. If the node is connected by some hnodes, those hnodes are recorded. 30 | ------------------------------------------------------- 31 | 5. Clear expired Xnode. Clear dead blocks. Update topological relationships. 32 | ``` 33 | 34 | -------------------------------------------------------------------------------- /Mapping/lowres_map/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lowres_map 4 | 0.0.0 5 | The lowres_map package 6 | 7 | 8 | 9 | CharlieDog 10 | 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | catkin 18 | catkin_simple 19 | 20 | roscpp 21 | tf 22 | visualization_msgs 23 | pcl_conversions 24 | pcl_ros 25 | 26 | sensor_msgs 27 | geometry_msgs 28 | block_map 29 | glog_catkin 30 | gflags_catkin 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /Test/ato_test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ato_test) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | add_definitions(-std=c++14) 6 | 7 | catkin_simple() 8 | 9 | cs_add_executable(fov_ray_test src/fov_ray_test.cpp) 10 | 11 | target_link_libraries(fov_ray_test 12 | ${catkin_LIBRARIES}) 13 | 14 | 15 | cs_add_executable(swarm_data_test src/swarm_data_test.cpp) 16 | 17 | target_link_libraries(swarm_data_test 18 | ${catkin_LIBRARIES}) 19 | 20 | cs_add_executable(traj_swarm src/traj_swarm.cpp) 21 | 22 | target_link_libraries(traj_swarm 23 | ${catkin_LIBRARIES}) 24 | 25 | cs_add_executable(cmd_test src/cmd_test.cpp) 26 | 27 | target_link_libraries(cmd_test 28 | ${catkin_LIBRARIES}) 29 | 30 | cs_add_executable(cmd_repub src/cmd_repub.cpp) 31 | 32 | target_link_libraries(cmd_repub 33 | ${catkin_LIBRARIES}) 34 | 35 | cs_add_executable(GMM_test src/GMM_test.cpp) 36 | 37 | target_link_libraries(cmd_repub 38 | ${catkin_LIBRARIES}) 39 | cs_install() 40 | cs_export() -------------------------------------------------------------------------------- /Test/ato_test/launch/cmd_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /Test/ato_test/launch/fov_ray.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /Test/ato_test/launch/swarm_sim/swarm_vis.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /Test/ato_test/launch/swarm_traj.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /Test/ato_test/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ato_test 4 | 0.0.0 5 | The ato_test package 6 | 7 | 8 | 9 | CharlieDog 10 | 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | catkin 18 | catkin_simple 19 | 20 | lowres_map 21 | multiDTG 22 | block_map 23 | geometry_msgs 24 | trajectory_msgs 25 | mav_msgs 26 | frontier_grid 27 | gflags_catkin 28 | visualization_msgs 29 | gazebo_msgs 30 | glog_catkin 31 | gcopter 32 | yaw_planner 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /Test/ato_test/resource/example.yaml: -------------------------------------------------------------------------------- 1 | Frontier/sample_max_range: 3.0 2 | Frontier/grid_scale: 2.0 3 | Frontier/viewpoint_thresh: 1.2 4 | Frontier/observe_thresh: 0.85 5 | Frontier/resample_duration: 0.8 6 | Frontier/sample_hor_dir_num: 10 7 | Frontier/sample_ver_dir_num: 3 8 | Frontier/sample_dist_num: 2 9 | Frontier/sensor_type: "Depth_Camera" 10 | Frontier/samp_free_thresh: 15 11 | Frontier/FOV_hor_num: 15 12 | Frontier/FOV_ver_num: 10 13 | # Frontier/sensor_type: "Livox" 14 | # Frontier/FOV_hor_num: 35 15 | # Frontier/FOV_ver_num: 10 16 | Frontier/cam_hor: 1.9 17 | Frontier/cam_ver: 1.046 18 | Frontier/livox_ver_low: -0.174 19 | Frontier/livox_ver_up: 1.308 20 | Frontier/ray_samp_dist1: 0.2 21 | Frontier/ray_samp_dist2: 0.1 22 | Frontier/show_frontier: true 23 | 24 | block_map/minX: -22.5 25 | block_map/minY: -22.5 26 | block_map/minZ: -0.1 27 | block_map/maxX: 22.5 28 | block_map/maxY: 22.5 29 | block_map/maxZ: 3.5 30 | block_map/blockX: 10 31 | block_map/blockY: 10 32 | block_map/blockZ: 10 33 | block_map/resolution: 0.1 34 | block_map/sensor_max_range: 5.0 35 | block_map/FFD: false 36 | block_map/depth: true 37 | block_map/update_freq: 10.0 38 | block_map/show_freq: 10.0 39 | block_map/depth_step: 2 40 | block_map/CamtoBody_Quater_x: -0.5 41 | block_map/CamtoBody_Quater_y: 0.5 42 | block_map/CamtoBody_Quater_z: -0.5 43 | block_map/CamtoBody_Quater_w: 0.5 44 | block_map/CamtoBody_x: 0.0 45 | block_map/CamtoBody_y: 0.0 46 | block_map/CamtoBody_z: 0.0 47 | block_map/occ_max: 0.90 48 | block_map/occ_min: 0.20 49 | block_map/pro_hit_occ: 0.90 50 | block_map/pro_miss_free: 0.75 51 | 52 | Exp/robot_sizeX: 0.5 53 | Exp/robot_sizeY: 0.5 54 | Exp/robot_sizeZ: 0.5 55 | Exp/minX: -20.0 56 | Exp/minY: -20.0 57 | Exp/minZ: 0.0 58 | Exp/maxX: 20.0 59 | Exp/maxY: 20.0 60 | Exp/maxZ: 3.0 61 | Exp/RobotId: 3 62 | Exp/UAV_id: 1 63 | 64 | MR_DTG/Toporange: 10.0 65 | MR_DTG/H_thresh: 5.5 66 | MR_DTG/update_FF: false 67 | MR_DTG/resX: 2.0 68 | MR_DTG/resY: 2.0 69 | MR_DTG/resZ: 2.0 70 | MR_DTG/show_edge_details: false 71 | 72 | LowResMap/localgraph_sizex: 14.0 73 | LowResMap/localgraph_sizey: 14.0 74 | LowResMap/localgraph_sizez: 8.0 75 | # LowResMap/minX: -20.0 76 | # LowResMap/minY: -20.0 77 | # LowResMap/minZ: 0.0 78 | # LowResMap/maxX: 20.0 79 | # LowResMap/maxY: 20.0 80 | # LowResMap/maxZ: 3.0 81 | LowResMap/node_x: 0.6 82 | LowResMap/node_y: 0.6 83 | LowResMap/node_z: 0.6 84 | LowResMap/blockX: 5 85 | LowResMap/blockY: 5 86 | LowResMap/blockZ: 3 87 | LowResMap/showmap: true 88 | LowResMap/debug: true 89 | LowResMap/lambda_heu: 5.0 90 | LowResMap/resolution: 0.1 91 | LowResMap/seg_length: 2.5 92 | LowResMap/prune_seg_length: 6.5 93 | LowResMap/occ_duration: 2.0 94 | LowResMap/unknown_duration: 0.2 95 | LowResMap/corridor_expX: 2 96 | LowResMap/corridor_expY: 2 97 | LowResMap/corridor_expZ: 1 98 | LowResMap/show_dtg: false 99 | 100 | -------------------------------------------------------------------------------- /Test/ato_test/resource/fov_cam.yaml: -------------------------------------------------------------------------------- 1 | Frontier/grid_scale: 2.0 2 | Frontier/viewpoint_thresh: 2.0 3 | Frontier/observe_thresh: 0.85 4 | Frontier/resample_duration: 1.0 5 | Frontier/sample_hor_dir_num: 8 6 | Frontier/sample_ver_dir_num: 3 7 | Frontier/sample_dist_num: 3 8 | Frontier/sensor_type: "Depth_Camera" 9 | Frontier/FOV_hor_num: 15 10 | Frontier/FOV_ver_num: 10 11 | # Frontier/sensor_type: "Livox" 12 | # Frontier/FOV_hor_num: 35 13 | # Frontier/FOV_ver_num: 10 14 | Frontier/cam_hor: 1.9 15 | Frontier/cam_ver: 1.046 16 | Frontier/livox_ver_low: -0.174 17 | Frontier/livox_ver_up: 1.308 18 | Frontier/ray_samp_dist1: 0.2 19 | Frontier/ray_samp_dist2: 0.1 20 | 21 | block_map/minX: -22.5 22 | block_map/minY: -22.5 23 | block_map/minZ: -0.1 24 | block_map/maxX: 22.5 25 | block_map/maxY: 22.5 26 | block_map/maxZ: 3.5 27 | block_map/blockX: 10 28 | block_map/blockY: 10 29 | block_map/blockZ: 10 30 | block_map/resolution: 0.1 31 | block_map/sensor_max_range: 5.0 32 | block_map/FFD: false 33 | block_map/depth: true 34 | block_map/update_freq: 10.0 35 | block_map/show_freq: 10.0 36 | block_map/depth_step: 2 37 | block_map/CamtoBody_Quater_x: -0.5 38 | block_map/CamtoBody_Quater_y: 0.5 39 | block_map/CamtoBody_Quater_z: -0.5 40 | block_map/CamtoBody_Quater_w: 0.5 41 | block_map/CamtoBody_x: 0.0 42 | block_map/CamtoBody_y: 0.0 43 | block_map/CamtoBody_z: 0.0 44 | block_map/occ_max: 0.90 45 | block_map/occ_min: 0.20 46 | block_map/pro_hit_occ: 0.90 47 | block_map/pro_miss_free: 0.75 48 | 49 | Exp/minX: -20.0 50 | Exp/minY: -20.0 51 | Exp/minZ: 0.0 52 | Exp/maxX: 20.0 53 | Exp/maxY: 20.0 54 | Exp/maxZ: 3.0 55 | Exp/RobotId: 3.0 56 | Exp/UAV_id: 1 57 | 58 | MR_DTG/Toporange: 10.0 59 | MR_DTG/H_thresh: 5.5 60 | MR_DTG/update_FF: false 61 | MR_DTG/resX: 2.0 62 | MR_DTG/resY: 2.0 63 | MR_DTG/resZ: 2.0 64 | MR_DTG/show_edge_details: true 65 | 66 | LowResMap/localgraph_sizex: 14.0 67 | LowResMap/localgraph_sizey: 14.0 68 | LowResMap/localgraph_sizez: 8.0 69 | # LowResMap/minX: -20.0 70 | # LowResMap/minY: -20.0 71 | # LowResMap/minZ: 0.0 72 | # LowResMap/maxX: 20.0 73 | # LowResMap/maxY: 20.0 74 | # LowResMap/maxZ: 3.0 75 | LowResMap/node_x: 0.6 76 | LowResMap/node_y: 0.6 77 | LowResMap/node_z: 0.6 78 | LowResMap/blockX: 5 79 | LowResMap/blockY: 5 80 | LowResMap/blockZ: 3 81 | LowResMap/showmap: true 82 | LowResMap/debug: true 83 | LowResMap/lambda_heu: 5.0 84 | LowResMap/resolution: 0.1 85 | LowResMap/seg_length: 2.5 86 | LowResMap/prune_seg_length: 6.5 87 | LowResMap/occ_duration: 2.0 88 | LowResMap/unknown_duration: 0.2 89 | LowResMap/corridor_expX: 2 90 | LowResMap/corridor_expY: 2 91 | LowResMap/corridor_expZ: 1 92 | 93 | -------------------------------------------------------------------------------- /Test/ato_test/resource/multi_robots_gazebo.yaml: -------------------------------------------------------------------------------- 1 | multisim/model_names: ["firefly1", "firefly2"] -------------------------------------------------------------------------------- /Test/ato_test/resource/multirobots.yaml: -------------------------------------------------------------------------------- 1 | Frontier/sample_max_range: 2.2 2 | Frontier/grid_scale: 2.0 3 | Frontier/viewpoint_thresh: 1.2 4 | Frontier/observe_thresh: 0.85 5 | Frontier/resample_duration: 0.8 6 | Frontier/sample_hor_dir_num: 10 7 | Frontier/sample_ver_dir_num: 3 8 | Frontier/sample_dist_num: 2 9 | Frontier/sensor_type: "Depth_Camera" 10 | Frontier/samp_free_thresh: 15 11 | Frontier/FOV_hor_num: 15 12 | Frontier/FOV_ver_num: 10 13 | # Frontier/sensor_type: "Livox" 14 | # Frontier/FOV_hor_num: 35 15 | # Frontier/FOV_ver_num: 10 16 | Frontier/cam_hor: 1.9 17 | Frontier/cam_ver: 1.046 18 | Frontier/livox_ver_low: -0.174 19 | Frontier/livox_ver_up: 1.308 20 | Frontier/ray_samp_dist1: 0.2 21 | Frontier/ray_samp_dist2: 0.1 22 | Frontier/show_frontier: true 23 | Frontier/min_vp_num: 6 24 | 25 | block_map/RobotVisX: 0.81 26 | block_map/RobotVisY: 0.81 27 | block_map/RobotVisZ: 0.41 28 | block_map/swarm_tol: 0.3 29 | block_map/swarm_send_delay: 2.0 30 | block_map/swarm_pub_thresh: 0.975 31 | block_map/minX: -22.5 32 | block_map/minY: -22.5 33 | block_map/minZ: -0.1 34 | block_map/maxX: 22.5 35 | block_map/maxY: 22.5 36 | block_map/maxZ: 3.5 37 | block_map/blockX: 10 38 | block_map/blockY: 10 39 | block_map/blockZ: 10 40 | block_map/resolution: 0.1 41 | block_map/sensor_max_range: 5.0 42 | block_map/FFD: false 43 | block_map/depth: true 44 | block_map/update_freq: 10.0 45 | block_map/show_freq: 10.0 46 | block_map/depth_step: 2 47 | block_map/CamtoBody_Quater_x: -0.5 48 | block_map/CamtoBody_Quater_y: 0.5 49 | block_map/CamtoBody_Quater_z: -0.5 50 | block_map/CamtoBody_Quater_w: 0.5 51 | block_map/CamtoBody_x: 0.0 52 | block_map/CamtoBody_y: 0.0 53 | block_map/CamtoBody_z: 0.0 54 | block_map/occ_max: 0.90 55 | block_map/occ_min: 0.20 56 | block_map/pro_hit_occ: 0.90 57 | block_map/pro_miss_free: 0.75 58 | block_map/statistic_v: true 59 | block_map/show_block: true 60 | block_map/min_finish_t: 50.0 61 | 62 | Exp/show_swarm_traj: false 63 | EXP/finish_hold_duration: 15.0 64 | Exp/robot_sizeX: 0.5 65 | Exp/robot_sizeY: 0.5 66 | Exp/robot_sizeZ: 0.5 67 | Exp/minX: -20.0 68 | Exp/minY: -20.0 69 | Exp/minZ: 0.0 70 | Exp/maxX: 20.0 71 | Exp/maxY: 20.0 72 | Exp/maxZ: 3.0 73 | Exp/traj_length: 10.0 74 | Exp/local_path_search: 1000 75 | Exp/global_path_search: 3000 76 | Exp/strong_check_interval: 3.0 77 | Exp/check_duration: 3.5 78 | Exp/exc_duration: 0.5 79 | Exp/replan_duration: 1.5 80 | Exp/takeoff_x: 0.0 81 | Exp/takeoff_y: 0.0 82 | Exp/takeoff_z: 1.0 83 | Exp/takeoff_yaw: 0.0 84 | Exp/reach_out_t: 0.1 85 | Exp/AutoTakeoff: true 86 | Exp/acc_off: -0.5 87 | Exp/statistic: true 88 | 89 | #swarm 90 | # Exp/UAV_id: 1 91 | # Exp/drone_num: 1 92 | Exp/local_comm_freq: 5.0 93 | Exp/global_comm_freq: 0.5 94 | Exp/local_dist_thresh: 7.0 95 | 96 | GVD/show_gvd: true 97 | GVD/lambda: 0.1 98 | GVD/allowance: 0.1 99 | GVD/tau: 0.3 100 | GVD/partition_frequency: 0.33 101 | 102 | MR_DTG/Toporange: 10.0 103 | MR_DTG/H_thresh: 5.5 104 | MR_DTG/update_FF: false 105 | MR_DTG/resX: 2.0 106 | MR_DTG/resY: 2.0 107 | MR_DTG/resZ: 2.0 108 | MR_DTG/show_edge_details: true 109 | 110 | LowResMap/localgraph_sizex: 14.0 111 | LowResMap/localgraph_sizey: 14.0 112 | LowResMap/localgraph_sizez: 8.0 113 | LowResMap/node_x: 0.6 114 | LowResMap/node_y: 0.6 115 | LowResMap/node_z: 0.6 116 | LowResMap/blockX: 5 117 | LowResMap/blockY: 5 118 | LowResMap/blockZ: 3 119 | LowResMap/showmap: true 120 | LowResMap/debug: true 121 | LowResMap/lambda_heu: 5.0 122 | LowResMap/resolution: 0.1 123 | LowResMap/seg_length: 2.5 124 | LowResMap/prune_seg_length: 6.5 125 | LowResMap/occ_duration: 2.0 126 | LowResMap/unknown_duration: 0.2 127 | LowResMap/corridor_expX: 2 128 | LowResMap/corridor_expY: 2 129 | LowResMap/corridor_expZ: 1 130 | LowResMap/show_dtg: false 131 | 132 | opt/YawVel: 2.1 133 | opt/YawAcc: 2.1 134 | opt/MaxVel: 1.5 135 | opt/MaxAcc: 1.5 136 | opt/SwarmAvoid: 5.5 137 | # opt/WeiPos: 5000.0 138 | # opt/WeiVel: 500.0 139 | # opt/WeiAcc: 100.0 140 | # opt/WeiSwarm: 3000.0 141 | # opt/WeiminT: 10.0 142 | # opt/WeiT: 200.0 143 | opt/WeiPos: 3000.0 144 | opt/WeiVel: 800.0 145 | opt/WeiAcc: 400.0 146 | opt/WeiSwarm: 500.0 147 | opt/WeiminT: 10.0 148 | opt/WeiT: 200.0 149 | opt/smoothingEps: 0.01 150 | opt/integralIntervs: 16 151 | opt/RelCostTolL: 1.0e-5 152 | 153 | Computation/data_num: 3 154 | Computation/dir: /home/charliedog/data/swarm/maze3/MURDER 155 | Computation/data_name0: gvp 156 | Computation/data_name1: local 157 | Computation/data_name2: global 158 | 159 | Computation/vdata_num: 9 160 | Computation/vdata_name0: volume 161 | Computation/vdata_name1: DTG 162 | Computation/vdata_name2: pose 163 | Computation/vdata_name3: traj 164 | Computation/vdata_name4: job 165 | Computation/vdata_name5: state 166 | Computation/vdata_name6: map 167 | Computation/vdata_name7: mapreq 168 | Computation/vdata_name8: total -------------------------------------------------------------------------------- /Test/ato_test/rviz/fov_gain.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | Splitter Ratio: 0.5 11 | Tree Height: 549 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: -1 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/Marker 56 | Enabled: true 57 | Marker Topic: /Frontier/debug 58 | Name: Marker 59 | Namespaces: 60 | "": true 61 | Queue Size: 100 62 | Value: true 63 | - Alpha: 1 64 | Class: rviz/Axes 65 | Enabled: true 66 | Length: 1 67 | Name: Axes 68 | Radius: 0.10000000149011612 69 | Reference Frame: 70 | Show Trail: false 71 | Value: true 72 | Enabled: true 73 | Global Options: 74 | Background Color: 48; 48; 48 75 | Default Light: true 76 | Fixed Frame: world 77 | Frame Rate: 30 78 | Name: root 79 | Tools: 80 | - Class: rviz/Interact 81 | Hide Inactive Objects: true 82 | - Class: rviz/MoveCamera 83 | - Class: rviz/Select 84 | - Class: rviz/FocusCamera 85 | - Class: rviz/Measure 86 | - Class: rviz/SetInitialPose 87 | Theta std deviation: 0.2617993950843811 88 | Topic: /initialpose 89 | X std deviation: 0.5 90 | Y std deviation: 0.5 91 | - Class: rviz/SetGoal 92 | Topic: /move_base_simple/goal 93 | - Class: rviz/PublishPoint 94 | Single click: true 95 | Topic: /clicked_point 96 | Value: true 97 | Views: 98 | Current: 99 | Class: rviz/Orbit 100 | Distance: 10 101 | Enable Stereo Rendering: 102 | Stereo Eye Separation: 0.05999999865889549 103 | Stereo Focal Distance: 1 104 | Swap Stereo Eyes: false 105 | Value: false 106 | Field of View: 0.7853981852531433 107 | Focal Point: 108 | X: 0 109 | Y: 0 110 | Z: 0 111 | Focal Shape Fixed Size: true 112 | Focal Shape Size: 0.05000000074505806 113 | Invert Z Axis: false 114 | Name: Current View 115 | Near Clip Distance: 0.009999999776482582 116 | Pitch: -0.09460125118494034 117 | Target Frame: 118 | Yaw: 1.1853958368301392 119 | Saved: ~ 120 | Window Geometry: 121 | Displays: 122 | collapsed: false 123 | Height: 846 124 | Hide Left Dock: false 125 | Hide Right Dock: false 126 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000034800000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 127 | Selection: 128 | collapsed: false 129 | Time: 130 | collapsed: false 131 | Tool Properties: 132 | collapsed: false 133 | Views: 134 | collapsed: false 135 | Width: 1200 136 | X: 72 137 | Y: 867 138 | -------------------------------------------------------------------------------- /Test/ato_test/rviz/swarm_traj.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /MarkerArray1 10 | Splitter Ratio: 0.5 11 | Tree Height: 719 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/MarkerArray 56 | Enabled: true 57 | Marker Topic: /traj 58 | Name: MarkerArray 59 | Namespaces: 60 | "": true 61 | Queue Size: 100 62 | Value: true 63 | - Alpha: 1 64 | Class: rviz/Axes 65 | Enabled: true 66 | Length: 1 67 | Name: Axes 68 | Radius: 0.10000000149011612 69 | Reference Frame: 70 | Show Trail: false 71 | Value: true 72 | Enabled: true 73 | Global Options: 74 | Background Color: 48; 48; 48 75 | Default Light: true 76 | Fixed Frame: world 77 | Frame Rate: 30 78 | Name: root 79 | Tools: 80 | - Class: rviz/Interact 81 | Hide Inactive Objects: true 82 | - Class: rviz/MoveCamera 83 | - Class: rviz/Select 84 | - Class: rviz/FocusCamera 85 | - Class: rviz/Measure 86 | - Class: rviz/SetInitialPose 87 | Theta std deviation: 0.2617993950843811 88 | Topic: /initialpose 89 | X std deviation: 0.5 90 | Y std deviation: 0.5 91 | - Class: rviz/SetGoal 92 | Topic: /move_base_simple/goal 93 | - Class: rviz/PublishPoint 94 | Single click: true 95 | Topic: /clicked_point 96 | Value: true 97 | Views: 98 | Current: 99 | Class: rviz/Orbit 100 | Distance: 22.90885353088379 101 | Enable Stereo Rendering: 102 | Stereo Eye Separation: 0.05999999865889549 103 | Stereo Focal Distance: 1 104 | Swap Stereo Eyes: false 105 | Value: false 106 | Field of View: 0.7853981852531433 107 | Focal Point: 108 | X: 0.23644104599952698 109 | Y: 1.114322304725647 110 | Z: 0.574303925037384 111 | Focal Shape Fixed Size: true 112 | Focal Shape Size: 0.05000000074505806 113 | Invert Z Axis: false 114 | Name: Current View 115 | Near Clip Distance: 0.009999999776482582 116 | Pitch: 1.4403976202011108 117 | Target Frame: 118 | Yaw: 4.840438365936279 119 | Saved: ~ 120 | Window Geometry: 121 | Displays: 122 | collapsed: false 123 | Height: 1016 124 | Hide Left Dock: false 125 | Hide Right Dock: false 126 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000034800000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 127 | Selection: 128 | collapsed: false 129 | Time: 130 | collapsed: false 131 | Tool Properties: 132 | collapsed: false 133 | Views: 134 | collapsed: false 135 | Width: 1848 136 | X: 72 137 | Y: 867 138 | -------------------------------------------------------------------------------- /Test/ato_test/src/GMM_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | int main(int argc, char** argv){ 24 | ros::init(argc, argv, "frontier_test"); 25 | ros::NodeHandle nh, nh_private("~"); 26 | 27 | google::InitGoogleLogging(argv[0]); 28 | google::ParseCommandLineFlags(&argc, &argv, true); 29 | google::InstallFailureSignalHandler(); 30 | 31 | // traj_pub_ = nh.advertise("/traj_repub", 5); 32 | // traj_sub_ = nh.subscribe("/trajectory_cmd", 1, &RepubCallback); 33 | 34 | ros::spin(); 35 | return 0; 36 | } -------------------------------------------------------------------------------- /Test/ato_test/src/cmd_repub.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | ros::Subscriber traj_sub_; 21 | ros::Publisher traj_pub_; 22 | bool have_init_t_ = false; 23 | double dt_; 24 | 25 | void RepubCallback(const swarm_exp_msgs::SwarmTrajConstPtr &traj){ 26 | if(!have_init_t_){ 27 | have_init_t_ = true; 28 | dt_ = ros::WallTime::now().toSec() - traj->start_t; 29 | } 30 | swarm_exp_msgs::SwarmTraj tr = *traj; 31 | tr.start_t += dt_; 32 | traj_pub_.publish(tr); 33 | } 34 | 35 | int main(int argc, char** argv){ 36 | ros::init(argc, argv, "frontier_test"); 37 | ros::NodeHandle nh, nh_private("~"); 38 | 39 | google::InitGoogleLogging(argv[0]); 40 | google::ParseCommandLineFlags(&argc, &argv, true); 41 | google::InstallFailureSignalHandler(); 42 | 43 | traj_pub_ = nh.advertise("/traj_repub", 5); 44 | traj_sub_ = nh.subscribe("/trajectory_cmd", 1, &RepubCallback); 45 | 46 | ros::spin(); 47 | return 0; 48 | } -------------------------------------------------------------------------------- /Test/ato_test/src/fov_ray_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | FrontierGrid *FG_; 9 | int main(int argc, char** argv){ 10 | ros::init(argc, argv, "fov_ray_test"); 11 | ros::NodeHandle nh, nh_private("~"); 12 | 13 | google::InitGoogleLogging(argv[0]); 14 | google::ParseCommandLineFlags(&argc, &argv, true); 15 | google::InstallFailureSignalHandler(); 16 | FrontierGrid FG; 17 | SwarmDataManager SDM_; 18 | 19 | BlockMap BM; 20 | BM.init(nh, nh_private); 21 | lowres::LowResMap LRM; 22 | 23 | ColorManager CM; 24 | CM.init(nh, nh_private); 25 | LRM.SetColorManager(&CM); 26 | 27 | LRM.SetMap(&BM); 28 | LRM.init(nh, nh_private); 29 | 30 | FG_ = &FG; 31 | FG_->SetMap(BM); 32 | FG_->SetLowresMap(LRM); 33 | FG_->SetSwarmDataManager(&SDM_); 34 | FG_->init(nh, nh_private); 35 | 36 | 37 | FG_->ShowGainDebug(); 38 | ros::spin(); 39 | return 0; 40 | } -------------------------------------------------------------------------------- /Test/ato_test/src/traj_swarm.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main() 6 | { 7 | 8 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 9 | 10 | pcl::io::loadPLYFile("/home/charliedog/rosprojects/MURDER/map2.ply", *cloud); 11 | 12 | 13 | pcl::io::savePCDFileBinary("/home/charliedog/rosprojects/MURDER/map.pcd", *cloud); 14 | 15 | system("pause"); 16 | 17 | return 0; 18 | } -------------------------------------------------------------------------------- /Test/ato_test/worlds/edgar_mine.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | model://edgar_mine 9 | 10 | 11 | 12 | EARTH_WGS84 13 | 47.3667 14 | 8.5500 15 | 500.0 16 | 0 17 | 18 | 19 | 20 | 21 | quick 22 | 1000 23 | 1.3 24 | 25 | 26 | 0 27 | 0.2 28 | 100 29 | 0.001 30 | 31 | 32 | 0.01 33 | 1 34 | 100 35 | 0 0 -9.8 36 | 37 | 38 | -------------------------------------------------------------------------------- /Test/ato_test/worlds/tunnel.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | 9 | model://sun 10 | 11 | 12 | model://tunnel 13 | 14 | 15 | 16 | EARTH_WGS84 17 | 47.3667 18 | 8.5500 19 | 500.0 20 | 0 21 | 22 | 23 | 24 | 25 | quick 26 | 1000 27 | 1.3 28 | 29 | 30 | 0 31 | 0.2 32 | 100 33 | 0.001 34 | 35 | 36 | 0.01 37 | 1 38 | 100 39 | 0 0 -9.8 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /Trajectory/gcopter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(gcopter) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | set(CMAKE_BUILD_TYPE "Release") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | # find_package(ompl REQUIRED) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | geometry_msgs 16 | sensor_msgs 17 | visualization_msgs 18 | ) 19 | 20 | include_directories( 21 | ${catkin_INCLUDE_DIRS} 22 | ${OMPL_INCLUDE_DIRS} 23 | ${EIGEN3_INCLUDE_DIRS} 24 | include 25 | ) 26 | 27 | catkin_package( 28 | INCLUDE_DIRS include 29 | LIBRARIES traj_opt 30 | # DEPENDS system_lib 31 | ) 32 | 33 | add_library(traj_opt src/traj_opt.cpp) 34 | 35 | target_link_libraries(traj_opt 36 | ${OMPL_LIBRARIES} 37 | ${catkin_LIBRARIES} 38 | ) 39 | -------------------------------------------------------------------------------- /Trajectory/gcopter/config/opt.yaml: -------------------------------------------------------------------------------- 1 | /opt/MaxVel: 2.0 2 | /opt/MaxAcc: 2.0 3 | /opt/WeiPos: 10000.0 4 | /opt/WeiVel: 1000.0 5 | /opt/WeiAcc: 1000.0 6 | /opt/WeiminT: 500.0 7 | /opt/WeiT: 100.0 8 | /opt/smoothingEps: 0.01 9 | /opt/integralIntervs: 16 10 | /opt/RelCostTolL: 1.0e-5 -------------------------------------------------------------------------------- /Trajectory/gcopter/include/gcopter/fastcheck_traj.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FASTCHECK_TRAJ_HPP 2 | #define FASTCHECK_TRAJ_HPP 3 | #include 4 | 5 | #include 6 | #include 7 | #include "gcopter/trajectory.hpp" 8 | 9 | class FastCheckTraj{ 10 | public: 11 | FastCheckTraj(){}; 12 | ~FastCheckTraj(){}; 13 | 14 | FastCheckTraj(Trajectory<5> &traj, const double &dt, const double &st){ 15 | // std::cout<<"FastCheckTraj0"<= traj_pts_.size() && traj_pts_.size() == 0) return false; 42 | else if(id >= traj_pts_.size()) pt = traj_pts_.back(); 43 | else pt = traj_pts_[id]; 44 | return true; 45 | } 46 | 47 | std::vector traj_pts_; 48 | double dt_; 49 | 50 | }; 51 | #endif -------------------------------------------------------------------------------- /Trajectory/gcopter/include/gcopter/traj_opt.h: -------------------------------------------------------------------------------- 1 | #include "gcopter/trajectory.hpp" 2 | #include "gcopter/gcopter.hpp" 3 | #include "gcopter/fastcheck_traj.hpp" 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | class AtoTraj{ 9 | public: 10 | void Init(ros::NodeHandle &nh, ros::NodeHandle &nh_private); 11 | bool Optimize( const vector &path, 12 | const vector &corridors, 13 | const vector &corridorVs, 14 | const double &min_t, 15 | const Eigen::Matrix3d &initState, 16 | const Eigen::Matrix3d &endState, 17 | const int &self_id, 18 | vector> &trajs, 19 | vector &swarm_t, 20 | vector &poses, 21 | double start_t); 22 | 23 | Trajectory<5> traj; 24 | vector weightVec_; //[pos, vel, acc, avoidT] 25 | vector upboundVec_; //[maxvel, maxacc, swarm] 26 | private: 27 | void Debug(list &debug_list); 28 | ros::Publisher swarm_traj_pub_; 29 | ros::NodeHandle nh_, nh_private_; 30 | double smoothingEps_; 31 | double weightT_, weight_minT_; 32 | int integralIntervs_; 33 | 34 | double trajlength_; 35 | 36 | double relCostTol_; 37 | 38 | double trajStamp; 39 | 40 | 41 | 42 | }; 43 | 44 | -------------------------------------------------------------------------------- /Trajectory/gcopter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | gcopter 3 | Zhepei Wang 4 | Zhepei Wang 5 | 0.1.0 6 | 7 | the gcopter package 8 | 9 | BSD 10 | 11 | catkin 12 | 13 | roscpp 14 | std_msgs 15 | geometry_msgs 16 | sensor_msgs 17 | visualization_msgs 18 | roscpp 19 | std_msgs 20 | geometry_msgs 21 | sensor_msgs 22 | visualization_msgs 23 | 24 | 25 | -------------------------------------------------------------------------------- /Trajectory/traj_exc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(traj_exc) 3 | 4 | add_definitions(-std=c++14) 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(catkin_simple REQUIRED) 8 | catkin_simple(ALL_DEPS_REQUIRED) 9 | 10 | include_directories( 11 | ${catkin_INCLUDE_DIRS} 12 | ) 13 | 14 | cs_add_executable(traj_exc_node src/traj_exc_node.cpp) 15 | 16 | target_link_libraries(traj_exc_node 17 | ${catkin_LIBRARIES}) 18 | ########## 19 | # EXPORT # 20 | ########## 21 | cs_install() 22 | cs_export() -------------------------------------------------------------------------------- /Trajectory/traj_exc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | traj_exc 4 | 0.0.0 5 | The traj_exc package 6 | 7 | 8 | 9 | CharlieDog 10 | 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | catkin 18 | catkin_simple 19 | 20 | roscpp 21 | visualization_msgs 22 | 23 | sensor_msgs 24 | geometry_msgs 25 | mav_msgs 26 | gcopter 27 | yaw_planner 28 | gflags_catkin 29 | glog_catkin 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /Trajectory/yaw_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(yaw_planner) 3 | 4 | add_definitions(-std=c++14) 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(catkin_simple REQUIRED) 8 | catkin_simple(ALL_DEPS_REQUIRED) 9 | 10 | include_directories( 11 | ${catkin_INCLUDE_DIRS} 12 | ) 13 | 14 | # cs_add_library(${PROJECT_NAME}_grid src/frontier_grid.cpp src/Viewpoint_manager.cpp) 15 | cs_add_library(${PROJECT_NAME} src/yaw_planner.cpp) 16 | 17 | ########## 18 | # EXPORT # 19 | ########## 20 | cs_install() 21 | cs_export() 22 | 23 | -------------------------------------------------------------------------------- /Trajectory/yaw_planner/include/yaw_planner/yaw_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef YAW_PLANNER_H_ 2 | #define YAW_PLANNER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | using namespace std; 14 | class YawPlanner{ 15 | public: 16 | void init(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); 17 | 18 | double GetMinT(const double &yaw_start, const double &yaw_end); 19 | bool Plan(const Eigen::VectorXd &yaw, Eigen::VectorXd &t, const double &vs, const double &as, 20 | const double &ve, const double &ae); 21 | void GetCmd(const double &t, double &yaw_p, double &yaw_v, double &yaw_a); 22 | void SampleT(const double &total_t, Eigen::VectorXd &t); 23 | double GetClosestYaw(const double &t, const double &yaw_s, const double &yaw_v, const double &yaw_t); 24 | 25 | /** 26 | * @brief 27 | * 28 | * @param yaw1 29 | * @param yaw2 30 | * @return get yaw distance [-PI, PI], yaw1 - yaw2 31 | */ 32 | inline double Dyaw(const double &yaw1, const double &yaw2); 33 | inline double Normyaw(const double &yaw); 34 | Eigen::VectorXd A_; 35 | Eigen::VectorXd T_; 36 | double v_max_, a_max_; 37 | 38 | private: 39 | 40 | double safe_t_; 41 | }; 42 | 43 | inline double YawPlanner::Normyaw(const double &yaw){ 44 | double yawn; 45 | int c = yaw / M_PI / 2; 46 | yawn = yaw - c * M_PI * 2; 47 | 48 | if(yawn < -M_PI) yawn += M_PI * 2; 49 | if(yawn > M_PI) yawn -= M_PI * 2; 50 | return yawn; 51 | } 52 | 53 | inline double YawPlanner::Dyaw(const double &yaw1, const double &yaw2){ 54 | return Normyaw(yaw1 - yaw2); 55 | } 56 | 57 | #endif -------------------------------------------------------------------------------- /Trajectory/yaw_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | yaw_planner 4 | 0.0.0 5 | The yaw_planner package 6 | 7 | 8 | 9 | 10 | CharlieDog 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | catkin 19 | catkin_simple 20 | 21 | roscpp 22 | visualization_msgs 23 | sensor_msgs 24 | nav_msgs 25 | swarm_exp_msgs 26 | geometry_msgs 27 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /data_statistics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(data_statistics) 3 | 4 | add_definitions(-std=c++14) 5 | set(CMAKE_BUILD_TYPE Release) 6 | 7 | find_package(catkin_simple REQUIRED) 8 | catkin_simple(ALL_DEPS_REQUIRED) 9 | include_directories( 10 | ${catkin_INCLUDE_DIRS} 11 | ) 12 | 13 | cs_add_library(computation_statistician src/computation_statistician.cpp) 14 | 15 | # cs_add_library(volum_listener src/volum_listener.cpp) 16 | # cs_add_executable(volum_logger src/volum_node.cpp) 17 | # target_link_libraries(volum_logger volum_listener) 18 | 19 | ########## 20 | # EXPORT # 21 | ########## 22 | cs_install() 23 | cs_export() 24 | -------------------------------------------------------------------------------- /data_statistics/include/data_statistics/computation_statistician.h: -------------------------------------------------------------------------------- 1 | #ifndef COMPUTATION_STATISTICIAN_H_ 2 | #define COMPUTATION_STATISTICIAN_H_ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | using namespace std; 9 | 10 | class ComputationStatistician{ 11 | public: 12 | ComputationStatistician(){}; 13 | ~ComputationStatistician(){ 14 | // oft_list_[i]<<"max:"< 0) v += volume_list_[i]; 18 | ofvdata_list_.back()< cost_list_; //total cost 47 | vector max_list_; 48 | vector cost_temp_list_; 49 | 50 | vector volume_list_; 51 | 52 | vector starttime_list_; 53 | vector name_list_; 54 | vector vname_list_; 55 | 56 | vector ofdata_list_; 57 | vector oft_list_; 58 | 59 | vector ofvdata_list_; 60 | vector ofvt_list_; 61 | string path_; 62 | int data_size_; 63 | int vdata_size_; 64 | ros::NodeHandle nh_private_, nh_; 65 | }; 66 | 67 | #endif -------------------------------------------------------------------------------- /data_statistics/launch/vt_statistician.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /data_statistics/launch/vts_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /data_statistics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | data_statistics 4 | 0.0.0 5 | The data_statistics package 6 | 7 | 8 | 9 | 10 | 11 | CharlieDog 12 | 13 | 14 | 15 | 16 | 17 | TODO 18 | 19 | catkin 20 | catkin_simple 21 | roscpp 22 | tf 23 | std_msgs 24 | nav_msgs 25 | sensor_msgs 26 | 27 | 28 | 29 | 30 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /data_statistics/src/computation_statistician.cpp: -------------------------------------------------------------------------------- 1 | #include "data_statistics/computation_statistician.h" 2 | 3 | 4 | void ComputationStatistician::init(ros::NodeHandle &nh, ros::NodeHandle &nh_private){ 5 | std::string ns = ros::this_node::getName(); 6 | nh_private_ = nh_private; 7 | int id, num; 8 | nh_private_.param(ns + "/Exp/UAV_id", 9 | id, 1); 10 | nh_private_.param(ns + "/Exp/drone_num", 11 | num, 1); 12 | nh_private_.param(ns + "/Computation/data_num", 13 | data_size_, 1); 14 | nh_private_.param(ns + "/Computation/vdata_num", 15 | vdata_size_, 1); 16 | nh_private_.param(ns + "/Computation/dir", dir_, dir_); 17 | // dir_ = dir_ + 18 | cost_list_.resize(data_size_); 19 | cost_temp_list_.resize(data_size_); 20 | starttime_list_.resize(data_size_); 21 | max_list_.resize(data_size_); 22 | name_list_.resize(data_size_); 23 | ofdata_list_.resize(data_size_); 24 | oft_list_.resize(data_size_); 25 | 26 | volume_list_.resize(vdata_size_, 0); 27 | ofvdata_list_.resize(vdata_size_); 28 | ofvt_list_.resize(vdata_size_); 29 | vname_list_.resize(vdata_size_); 30 | 31 | time_t now = time(0); 32 | std::string Time_ = ctime(&now); 33 | tm* t=localtime(&now); 34 | string command; 35 | path_ = dir_+"/"+to_string(num)+"/"+to_string(t->tm_year+1900)+"_"+to_string(t->tm_mon+1)+"_"+to_string(t->tm_mday) 36 | +"_"+to_string(t->tm_hour)+"_"+to_string(t->tm_min)+"_"+to_string(t->tm_sec)+"_"+to_string(id); 37 | command = "mkdir "+path_; 38 | cout<<"command:"<= data_size_){ 62 | printf("\033[0;31m ERROR dataid! dataid should be less than %d\033[0m\n", dataid); 63 | return; 64 | } 65 | starttime_list_[dataid] = ros::WallTime::now(); 66 | } 67 | 68 | void ComputationStatistician::EndTimer(int dataid){ 69 | if(dataid < 0 || dataid >= data_size_){ 70 | printf("\033[0;31m ERROR dataid! The dataid should be less than %d\033[0m\n", data_size_); 71 | return; 72 | } 73 | double cost = (ros::WallTime::now() - starttime_list_[dataid]).toSec(); 74 | double t = ros::WallTime::now().toSec() - start_t_; 75 | if(cost > max_list_[dataid]) max_list_[dataid] = cost; 76 | cost_temp_list_[dataid] = cost; 77 | cost_list_[dataid] += cost; 78 | ofdata_list_[dataid]<= vdata_size_){ 85 | printf("\033[0;31m ERROR dataid! The dataid should be less than %d\033[0m\n", vdata_size_); 86 | return; 87 | } 88 | double t = ros::WallTime::now().toSec() - start_t_; 89 | volume_list_[id] += v; 90 | ofvdata_list_[id]<= vdata_size_){ 96 | printf("\033[0;31m ERROR dataid! The dataid should be less than %d\033[0m\n", vdata_size_); 97 | return; 98 | } 99 | double t = ros::WallTime::now().toSec() - start_t_; 100 | // ROS_ERROR("id:%d %d",id, vdata_size_); 101 | volume_list_[id] = v; 102 | ofvdata_list_[id]<= data_size_){ 107 | // printf("\033[0;31m ERROR dataid! The dataid should be less than %d\033[0m\n", data_size_); 108 | // } 109 | // printf("\033[0;32m %s spend %lfs\033[0m\n", name_list_[dataid].c_str(), cost_list_[dataid]); 110 | // } 111 | 112 | // void ComputationStatistician::ShowCost(int dataid){ 113 | // if(dataid < 0 || dataid >= data_size_){ 114 | // printf("\033[0;31m ERROR dataid! The dataid should be less than %d\033[0m\n", data_size_); 115 | // } 116 | // cost_temp_list_[dataid] = (starttime_list_[dataid] - ros::WallTime::now()).toSec(); 117 | // printf("\033[0;32m %s spend %lfs\033[0m\n", name_list_[dataid].c_str(), cost_temp_list_[dataid]); 118 | // } 119 | 120 | // void ComputationStatistician::ShowAllCost(){ 121 | // for(int i = 0; i < data_size_; i++){ 122 | // printf("\033[0;32m %s spend %lfs\033[0m\n", name_list_[i].c_str(), cost_temp_list_[i]); 123 | // } 124 | // } -------------------------------------------------------------------------------- /doing/DTG-based-planning2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/doing/DTG-based-planning2.pdf -------------------------------------------------------------------------------- /doing/DTG-based-planning3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/doing/DTG-based-planning3.pdf -------------------------------------------------------------------------------- /doing/Frontier_communication.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/doing/Frontier_communication.pdf -------------------------------------------------------------------------------- /doing/experiment.txt: -------------------------------------------------------------------------------- 1 | 1. FUEL + ETHZ 两种方法gazebo环境（rotors-modified） 2 | 2. benchmark转移到DIY环境（再装NUC） 3 | 3. 跑数据 4 | -------------------------------------------------------------------------------- /doing/local_voronoi_partition.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/doing/local_voronoi_partition.pdf -------------------------------------------------------------------------------- /doing/todo.txt: -------------------------------------------------------------------------------- 1 | 1. blockmap 的体素和frontier origin对齐 2 | 2. Coarse Expand 在地图边界报错 3 | [ WARN] [1715840627.584242218, 4727.606000000]: id:8 local plan! 4 | id:8hn_l:3 lowbd:13.9041 -11.655 -3.196 upbd:29.4041 3.84498 12.304 5 | id:8exp_state:0 24.1953 -5.595 2.67691 2.51327 6 | *** Aborted at 1715840627 (unix time) try "date -d @1715840627" if you are using GNU date *** 7 | PC: @ 0x7f517a2ce758 lowres::LowResMap::CoarseExpand() 8 | *** SIGSEGV (@0x2a59) received by PID 132318 (TID 0x7f516b569640) from PID 10841; stack trace: *** 9 | @ 0x7f517ae78631 (unknown) 10 | @ 0x7f517aea5420 (unknown) 11 | @ 0x7f517a2ce758 lowres::LowResMap::CoarseExpand() 12 | @ 0x7f517a2db8ee lowres::LowResMap::FindCorridors() 13 | @ 0x7f517a72e2ae Murder::TrajPlanB() 14 | @ 0x7f517a73103b Murder::LocalPlan() 15 | 38 63 2 -1 16 | map_upbd_:24.7999 24.7999 6.3999 17 | map_lowbd_:-25.5999 -25.5999 -1.5999 18 | 19 | 3. Gain area cos_phi needs to be modified, and the parameter needs to be tuned -------------------------------------------------------------------------------- /pics/0.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/pics/0.gif -------------------------------------------------------------------------------- /pics/1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/pics/1.gif -------------------------------------------------------------------------------- /pics/cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/pics/cover.png -------------------------------------------------------------------------------- /pics/cover_new.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NKU-MobFly-Robotics/GVP-MREP/24ee1e09b0c6149ced639eea6eeadc35bca3445d/pics/cover_new.png --------------------------------------------------------------------------------