├── msg └── PointArray.msg ├── others ├── rosgraph.png └── record.sh ├── param ├── dwa │ ├── local_costmap_params.yaml │ ├── global_costmap_params.yaml │ ├── costmap_common_params.yaml │ └── local_planner_params.yaml ├── teb │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ ├── costmap_common_params.yaml │ ├── costmap_converter_params.yaml │ └── local_planner_params.yaml ├── tdle.lua └── rviz_config │ └── single.rviz ├── .gitignore ├── launch ├── tdle_real.launch ├── include │ ├── basic_exploration.launch │ ├── robot.launch │ ├── move_base_real.launch │ └── move_base_sim.launch └── tdle_sim.launch ├── include └── tdle │ ├── data_recorder_node.h │ ├── fpoint_detector_node.h │ ├── fpoint_selection_node.h │ ├── common_func.h │ ├── fpoint_filter_node.h │ └── subregion_arrangement_node.h ├── package.xml ├── CMakeLists.txt ├── src ├── data_recorder_node.cpp ├── common_func.cpp ├── fpoint_detector_node.cpp ├── fpoint_filter_node.cpp ├── fpoint_selection_node.cpp └── subregion_arrangement_node.cpp └── README.md /msg/PointArray.msg: -------------------------------------------------------------------------------- 1 | # An array of points 2 | 3 | geometry_msgs/Point[] points -------------------------------------------------------------------------------- /others/rosgraph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SeanZsya/tdle/HEAD/others/rosgraph.png -------------------------------------------------------------------------------- /others/record.sh: -------------------------------------------------------------------------------- 1 | rosbag record -o ~/ /map /centroids /grid_marker /subregion_marker /trajectory_node_list /move_base/NavfnROS/plan /global_shapes /local_shapes -------------------------------------------------------------------------------- /param/dwa/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | 3 | global_frame: map 4 | robot_base_frame: base_link 5 | update_frequency: 3.0 6 | publish_frequency: 3.0 7 | static_map: false 8 | rolling_window: true 9 | 10 | width: 1.5 11 | height: 1.5 12 | resolution: 0.01 13 | -------------------------------------------------------------------------------- /param/dwa/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | 3 | global_frame: map 4 | robot_base_frame: base_link 5 | update_frequency: 3.0 6 | publish_frequency: 1.0 7 | transform_tolerance: 5 8 | static_map: true 9 | rolling_window: true 10 | 11 | width: 100.0 12 | height: 100.0 13 | resolution: 0.05 14 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | .vscode/ -------------------------------------------------------------------------------- /param/teb/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 1.0 5 | publish_frequency: 0.5 6 | static_map: true 7 | 8 | transform_tolerance: 0.5 9 | plugins: 10 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 11 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 12 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} -------------------------------------------------------------------------------- /param/teb/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 5.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 5.5 9 | height: 5.5 10 | resolution: 0.1 11 | transform_tolerance: 0.5 12 | 13 | plugins: 14 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 15 | - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"} 16 | -------------------------------------------------------------------------------- /launch/tdle_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /include/tdle/data_recorder_node.h: -------------------------------------------------------------------------------- 1 | #ifndef DATA_RECORDER_NODE_H 2 | #define DATA_RECORDER_NODE_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 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | #endif -------------------------------------------------------------------------------- /include/tdle/fpoint_detector_node.h: -------------------------------------------------------------------------------- 1 | #ifndef FPOINT_DETECTOR_NODE_H 2 | #define FPOINT_DETECTOR_NODE_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 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | #endif -------------------------------------------------------------------------------- /param/dwa/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | # #This file contains common configuration options for the two costmaps used in the navigation stack for more details on the parameters in this file, and a full list of the parameters used by the costmaps, please see http://www.ros.org/wiki/costmap_2d 2 | 3 | # lethal_cost_threshold: 100 4 | 5 | obstacle_range: 3.0 6 | raytrace_range: 3.5 7 | 8 | footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]] 9 | #robot_radius: 0.17 10 | 11 | inflation_radius: 0.6 #0.6 12 | cost_scaling_factor: 4.0 #4.0 13 | 14 | map_type: costmap 15 | observation_sources: scan 16 | scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true} 17 | -------------------------------------------------------------------------------- /include/tdle/fpoint_selection_node.h: -------------------------------------------------------------------------------- 1 | #ifndef FPOINT_SELECTION_NODE_H 2 | #define FPOINT_SELECTION_NODE_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 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | 24 | enum EXP_STATE { NORMAL, STUCKED, FINISHED }; 25 | 26 | 27 | #endif -------------------------------------------------------------------------------- /launch/include/basic_exploration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/include/robot.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /param/teb/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | 2 | #---standard pioneer footprint--- 3 | #---(in meters)--- 4 | robot_radius: 0.17 5 | footprint_padding: 0.00 6 | 7 | transform_tolerance: 0.2 8 | map_type: costmap 9 | 10 | always_send_full_costmap: true 11 | 12 | obstacle_layer: 13 | enabled: true 14 | obstacle_range: 3.0 15 | raytrace_range: 4.0 16 | inflation_radius: 0.2 17 | track_unknown_space: true 18 | combination_method: 1 19 | 20 | observation_sources: laser_scan_sensor 21 | laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true} 22 | 23 | 24 | inflation_layer: 25 | enabled: true 26 | cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10) 27 | inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths. 28 | 29 | static_layer: 30 | enabled: true 31 | map_topic: "/map" 32 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tdle 4 | 1.0.0 5 | A ROS package for exploration with 2d lidar. 6 | 7 | Sean Zhao 8 | Sean Zhao 9 | 10 | MIT 11 | https://github.com/SeanZsya/tdle 12 | 13 | 14 | catkin 15 | 16 | geometry_msgs 17 | nav_msgs 18 | roscpp 19 | std_msgs 20 | visualization_msgs 21 | message_generation 22 | 23 | geometry_msgs 24 | nav_msgs 25 | roscpp 26 | std_msgs 27 | visualization_msgs 28 | message_runtime 29 | 30 | gazebo_ros 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /include/tdle/common_func.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_FUNC_H 2 | #define COMMON_FUNC_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 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | 23 | float euclideanDistance( vector , vector ); 24 | 25 | float sign(float ); 26 | 27 | vector Nearest(vector >,vector ); 28 | 29 | vector Steer( vector, vector, float ); 30 | 31 | int gridValue(nav_msgs::OccupancyGrid &,vector); 32 | 33 | int ObstacleFree(vector , vector & , nav_msgs::OccupancyGrid); 34 | 35 | void visMarkerInit(visualization_msgs::Marker &marker, string ID, int type, int action, float scale, float r, float g, float b, float a); 36 | 37 | void visMarkerSet (visualization_msgs::Marker &marker, vector &point); 38 | 39 | 40 | #endif -------------------------------------------------------------------------------- /include/tdle/fpoint_filter_node.h: -------------------------------------------------------------------------------- 1 | #ifndef FPOINT_FILTER_NODE_H 2 | #define FPOINT_FILTER_NODE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | using namespace std; 23 | 24 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg); 25 | 26 | void costmapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg); 27 | 28 | void fpointCallBack(const geometry_msgs::PointStamped& msg); 29 | 30 | vector disFilter(geometry_msgs::Point point, vector& fpoints); 31 | 32 | vector meanshiftFilter(geometry_msgs::Point point, vector &fpoints, float kernel_bandwidth, float convergence_threshold); 33 | 34 | tdle::PointArray delPrevious(vector& filtered_points, visualization_msgs::Marker& fpoint_vis) ; 35 | 36 | 37 | 38 | #endif -------------------------------------------------------------------------------- /launch/tdle_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 | -------------------------------------------------------------------------------- /param/dwa/local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | # Robot Configuration Parameters 3 | max_vel_x: 1 4 | min_vel_x: -1 5 | 6 | max_vel_y: 0.0 7 | min_vel_y: 0.0 8 | 9 | max_vel_theta: 3.0 #max_rot_vel is deprecated 10 | min_vel_theta: -3.0 11 | 12 | #平移速度 13 | max_vel_trans: 1 14 | min_vel_trans: -1 15 | 16 | acc_lim_x: 4.5 17 | acc_lim_y: 0.0 18 | acc_lim_th: 2.0 19 | 20 | 21 | 22 | # Goal Tolerance Parametes 23 | xy_goal_tolerance: 0.10 24 | yaw_goal_tolerance: 1.6 #1.0 25 | latch_xy_goal_tolerance: false 26 | #If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so. 27 | 28 | 29 | 30 | # Forward Simulation Parameters 31 | sim_time: 1.2 #1.2 32 | vx_samples: 20 33 | sim_granularity: 0.025 34 | #The step size, in meters, to take between points on a given trajectory 35 | vy_samples: 0 36 | vth_samples: 40 37 | controller_frequency: 10.0 38 | 39 | # Trajectory Scoring Parameters 40 | path_distance_bias: 32.0 41 | goal_distance_bias: 24.0 42 | occdist_scale: 0.05 #0.2 43 | #The weighting for how much the controller should attempt to avoid obstacles 44 | forward_point_distance: 0.325 45 | stop_time_buffer: 0.2 46 | scaling_speed: 0.25 47 | max_scaling_factor: 0.2 48 | 49 | # Oscillation Prevention Parameters 50 | oscillation_reset_dist: 0.05 51 | 52 | # Debugging 53 | publish_traj_pc : true 54 | publish_cost_grid_pc: true 55 | -------------------------------------------------------------------------------- /param/teb/costmap_converter_params.yaml: -------------------------------------------------------------------------------- 1 | ########################################################################################### 2 | ## NOTE: Costmap conversion is experimental. Its purpose is to combine many point ## 3 | ## obstales into clusters, computed in a separate thread in order to improve the overall ## 4 | ## efficiency of local planning. However, the implemented conversion algorithms are in a ## 5 | ## very early stage of development. Contributions are welcome! ## 6 | ########################################################################################### 7 | 8 | TebLocalPlannerROS: 9 | 10 | ## Costmap converter plugin 11 | #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" 12 | costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC" 13 | #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH" 14 | #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull" 15 | costmap_converter_spin_thread: True 16 | costmap_converter_rate: 5 17 | 18 | 19 | ## Configure plugins (namespace move_base/costmap_to_lines or move_base/costmap_to_polygons) 20 | ## costmap_converter/CostmapToLinesDBSRANSAC, costmap_converter/CostmapToLinesDBSMCCH, costmap_converter/CostmapToPolygonsDBSMCCH 21 | costmap_converter/CostmapToLinesDBSRANSAC: 22 | cluster_max_distance: 0.4 23 | cluster_min_pts: 2 24 | ransac_inlier_distance: 0.15 25 | ransac_min_inliers: 10 26 | ransac_no_iterations: 1500 27 | ransac_remainig_outliers: 3 28 | ransac_convert_outlier_pts: True 29 | ransac_filter_remaining_outlier_pts: False 30 | convex_hull_min_pt_separation: 0.1 31 | 32 | -------------------------------------------------------------------------------- /include/tdle/subregion_arrangement_node.h: -------------------------------------------------------------------------------- 1 | #ifndef SUBREGION_ARRANGEMENT_NODE_H 2 | #define SUBREGION_ARRANGEMENT_NODE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | using namespace std; 27 | 28 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg); 29 | 30 | void odomCallBack(const nav_msgs::Odometry& msg); 31 | 32 | void fpCallBack(const tdle::PointArray& msg); 33 | 34 | void drawSubRegi(vector serial, ros::Publisher marker_pub); 35 | 36 | void drawGrid(ros::Publisher marker_pub); 37 | 38 | bool isInside(geometry_msgs::Point center, geometry_msgs::Point point); 39 | 40 | bool isUnknown(geometry_msgs::Point center); 41 | 42 | vector selectSubRegi(); 43 | 44 | vector sortSubRgi_tsp(int full_graph[10][10], int x_bot_grid, vector selected_sr); 45 | 46 | vector sortSubRgi_bfs(vector> full_graph,vector tbd_waypoints,vector previous_path,int sr_bot); 47 | 48 | float similarity_dtw(vector> full_graph, vector route1, vector route2); 49 | 50 | vector> classifyFpoints(vector sr_sorted); 51 | 52 | float navi_cost(geometry_msgs::Point next_center, geometry_msgs::Point fpoint); 53 | 54 | #endif -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(tdle) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | nav_msgs 7 | roscpp 8 | std_msgs 9 | actionlib 10 | visualization_msgs 11 | message_generation 12 | ) 13 | 14 | add_message_files( 15 | FILES 16 | PointArray.msg 17 | ) 18 | 19 | generate_messages( 20 | DEPENDENCIES 21 | std_msgs 22 | geometry_msgs 23 | ) 24 | 25 | catkin_package( 26 | CATKIN_DEPENDS message_runtime 27 | ) 28 | 29 | include_directories( 30 | include 31 | SYSTEM 32 | ${catkin_INCLUDE_DIRS} 33 | ${PROJECT_SOURCE_DIR}/include 34 | ) 35 | 36 | add_executable( 37 | fpoint_detector 38 | src/fpoint_detector_node.cpp 39 | src/common_func.cpp) 40 | target_link_libraries(fpoint_detector ${catkin_LIBRARIES}) 41 | add_dependencies(fpoint_detector ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 42 | 43 | add_executable( 44 | fpoint_filter 45 | src/fpoint_filter_node.cpp 46 | src/common_func.cpp) 47 | target_link_libraries(fpoint_filter ${catkin_LIBRARIES}) 48 | add_dependencies(fpoint_filter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 49 | 50 | add_executable( 51 | subregion_arrangement 52 | src/subregion_arrangement_node.cpp 53 | src/common_func.cpp 54 | ) 55 | target_link_libraries(subregion_arrangement ${catkin_LIBRARIES}) 56 | add_dependencies(subregion_arrangement ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 57 | 58 | add_executable( 59 | fpoint_selection 60 | src/fpoint_selection_node.cpp 61 | src/common_func.cpp) 62 | target_link_libraries(fpoint_selection ${catkin_LIBRARIES}) 63 | add_dependencies(fpoint_selection ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 64 | 65 | add_executable( 66 | data_recorder 67 | src/data_recorder_node.cpp 68 | src/common_func.cpp) 69 | target_link_libraries(data_recorder ${catkin_LIBRARIES}) 70 | add_dependencies(data_recorder ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -------------------------------------------------------------------------------- /src/data_recorder_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | nav_msgs::Odometry odom; 10 | nav_msgs::OccupancyGrid mapData; 11 | geometry_msgs::Point map_origin, next_center; 12 | float map_width, map_height; 13 | 14 | 15 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) 16 | { 17 | mapData = *msg; 18 | map_width = msg->info.width * msg->info.resolution; 19 | map_height = msg->info.height * msg->info.resolution; 20 | map_origin = msg->info.origin.position; 21 | } 22 | 23 | void odomCallBack(const nav_msgs::Odometry& msg) 24 | { 25 | odom = msg; 26 | } 27 | 28 | int main(int argc, char **argv) 29 | { 30 | ros::init(argc, argv, "FPonit_Assigner"); 31 | ros::NodeHandle nh; 32 | 33 | string map_topic, odom_topic; 34 | 35 | ros::param::param("/map_topic", map_topic, "/map"); 36 | ros::param::param("/odom_topic", odom_topic, "/odom"); 37 | ros::Subscriber mapsub = nh.subscribe(map_topic, 10, mapCallBack); 38 | ros::Subscriber odomsub = nh.subscribe(odom_topic, 10, odomCallBack); 39 | 40 | 41 | ros::Rate rate(1); // run at 1Hz 42 | 43 | // wait until map is received 44 | while (mapData.header.seq<1 or mapData.data.size()<1) {ros::spinOnce(); rate.sleep();} 45 | 46 | // Get current time 47 | time_t currentTime; 48 | time(¤tTime); 49 | 50 | // Convert current time to string 51 | char* timeString = ctime(¤tTime); 52 | timeString[strlen(timeString) - 1] = '\0'; 53 | // Create filename with current time 54 | string filePath = "/home/sean/tdle_data/"; 55 | string filename = filePath + string(timeString) + ".txt"; 56 | 57 | // save robot position and map size to a txt file, name with current time 58 | ofstream outputFile(filename); 59 | 60 | // Main loop 61 | while (ros::ok()) 62 | { 63 | outputFile << odom.pose.pose.position.x << " " << odom.pose.pose.position.y << " " << map_width * map_height << endl; 64 | 65 | ros::spinOnce(); 66 | rate.sleep(); 67 | } 68 | outputFile.close(); 69 | return 0; 70 | } -------------------------------------------------------------------------------- /param/tdle.lua: -------------------------------------------------------------------------------- 1 | -- Copyright 2016 The Cartographer Authors 2 | -- 3 | -- Licensed under the Apache License, Version 2.0 (the "License"); 4 | -- you may not use this file except in compliance with the License. 5 | -- You may obtain a copy of the License at 6 | -- 7 | -- http://www.apache.org/licenses/LICENSE-2.0 8 | -- 9 | -- Unless required by applicable law or agreed to in writing, software 10 | -- distributed under the License is distributed on an "AS IS" BASIS, 11 | -- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | -- See the License for the specific language governing permissions and 13 | -- limitations under the License. 14 | 15 | include "map_builder.lua" 16 | include "trajectory_builder.lua" 17 | 18 | options = { 19 | map_builder = MAP_BUILDER, 20 | trajectory_builder = TRAJECTORY_BUILDER, 21 | map_frame = "map", 22 | tracking_frame = "base_footprint", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) 23 | published_frame = "odom", 24 | odom_frame = "odom", 25 | provide_odom_frame = false, 26 | publish_frame_projected_to_2d = false, 27 | use_odometry = true, 28 | use_nav_sat = false, 29 | use_landmarks = false, 30 | num_laser_scans = 1, 31 | num_multi_echo_laser_scans = 0, 32 | num_subdivisions_per_laser_scan = 1, 33 | num_point_clouds = 0, 34 | lookup_transform_timeout_sec = 0.2, 35 | submap_publish_period_sec = 0.3, 36 | pose_publish_period_sec = 5e-3, 37 | trajectory_publish_period_sec = 30e-3, 38 | rangefinder_sampling_ratio = 1., 39 | odometry_sampling_ratio = 1., 40 | fixed_frame_pose_sampling_ratio = 1., 41 | imu_sampling_ratio = 1., 42 | landmarks_sampling_ratio = 1., 43 | } 44 | 45 | MAP_BUILDER.use_trajectory_builder_2d = true 46 | 47 | TRAJECTORY_BUILDER_2D.min_range = 0.1 48 | TRAJECTORY_BUILDER_2D.max_range = 15 49 | TRAJECTORY_BUILDER_2D.missing_data_ray_length = 0. 50 | TRAJECTORY_BUILDER_2D.use_imu_data = false --main diffderence from before 51 | TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 52 | TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) 53 | 54 | POSE_GRAPH.constraint_builder.min_score = 0.65 55 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 56 | 57 | return options 58 | -------------------------------------------------------------------------------- /param/teb/local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TebLocalPlannerROS: 2 | 3 | odom_topic: odom 4 | 5 | # Trajectory 6 | 7 | teb_autosize: True 8 | dt_ref: 0.3 9 | dt_hysteresis: 0.1 10 | max_samples: 500 11 | global_plan_overwrite_orientation: True 12 | allow_init_with_backwards_motion: False 13 | max_global_plan_lookahead_dist: 3.0 14 | global_plan_viapoint_sep: -1 15 | global_plan_prune_distance: 1 16 | exact_arc_length: False 17 | feasibility_check_no_poses: 5 18 | publish_feedback: False 19 | 20 | # Robot 21 | 22 | max_vel_x: 1.0 23 | max_vel_x_backwards: 1.0 24 | max_vel_y: 0.0 25 | max_vel_theta: 0.5 26 | acc_lim_x: 0.3 27 | acc_lim_theta: 4.0 28 | min_turning_radius: 0.0 # diff-drive robot (can turn on place!) 29 | 30 | footprint_model: 31 | type: "point" 32 | 33 | # GoalTolerance 34 | 35 | xy_goal_tolerance: 0.3 36 | yaw_goal_tolerance: 1.2 37 | free_goal_vel: True 38 | complete_global_plan: True 39 | 40 | # Obstacles 41 | 42 | min_obstacle_dist: 0.25 # This value must also include our robot radius, since footprint_model is set to "point". 43 | inflation_dist: 0.6 44 | include_costmap_obstacles: True 45 | costmap_obstacles_behind_robot_dist: 1.5 46 | obstacle_poses_affected: 15 47 | 48 | dynamic_obstacle_inflation_dist: 0.6 49 | include_dynamic_obstacles: True 50 | 51 | costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC" 52 | costmap_converter_spin_thread: True 53 | costmap_converter_rate: 5 54 | 55 | # Optimization 56 | 57 | no_inner_iterations: 5 58 | no_outer_iterations: 4 59 | optimization_activate: True 60 | optimization_verbose: False 61 | penalty_epsilon: 0.1 62 | obstacle_cost_exponent: 4 63 | weight_max_vel_x: 2 64 | weight_max_vel_theta: 1 65 | weight_acc_lim_x: 1 66 | weight_acc_lim_theta: 1 67 | weight_kinematics_nh: 1000 68 | weight_kinematics_forward_drive: 0.0 69 | weight_kinematics_turning_radius: 1 70 | weight_optimaltime: 1 # must be > 0 71 | weight_shortest_path: 0 72 | weight_obstacle: 100 73 | weight_inflation: 0.2 74 | weight_dynamic_obstacle: 10 75 | weight_dynamic_obstacle_inflation: 0.2 76 | weight_viapoint: 1 77 | weight_adapt_factor: 2 78 | 79 | # Homotopy Class Planner 80 | 81 | enable_homotopy_class_planning: False 82 | enable_multithreading: True 83 | max_number_classes: 4 84 | selection_cost_hysteresis: 1.0 85 | selection_prefer_initial_plan: 0.9 86 | selection_obst_cost_scale: 100.0 87 | selection_alternative_time_cost: False 88 | 89 | roadmap_graph_no_samples: 15 90 | roadmap_graph_area_width: 5 91 | roadmap_graph_area_length_scale: 1.0 92 | h_signature_prescaler: 0.5 93 | h_signature_threshold: 0.1 94 | obstacle_heading_threshold: 0.45 95 | switching_blocking_period: 0.0 96 | viapoints_all_candidates: True 97 | delete_detours_backwards: True 98 | max_ratio_detours_duration_best_duration: 3.0 99 | visualize_hc_graph: False 100 | visualize_with_time_as_z_axis_scale: False 101 | 102 | # Recovery 103 | 104 | shrink_horizon_backup: True 105 | shrink_horizon_min_duration: 10 106 | oscillation_recovery: True 107 | oscillation_v_eps: 0.1 108 | oscillation_omega_eps: 0.1 109 | oscillation_recovery_min_duration: 10 110 | oscillation_filter_duration: 10 111 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TDLE 2 | 3 | [The corresponding paper ***[TDLE: 2D Lidar Exploration with Hierarchical Planning Using Regional Division](https://arxiv.org/pdf/2307.02852.pdf)*** has been accepted by IEEE CASE 2023.] 4 | 5 | 6 | ## Introduction 7 | 8 | Demonstration Video: 9 | 10 | [![TDLE: 2D Lidar Exploration with Hierarchical Planning Using Regional Division](https://res.cloudinary.com/marcomontalbano/image/upload/v1685324236/video_to_markdown/images/youtube--aPXxOKf1o10-c05b58ac6eb4c4700831b2b3070cd403.jpg)](https://youtu.be/aPXxOKf1o10 "TDLE: 2D Lidar Exploration with Hierarchical Planning Using Regional Division") 11 | 12 | ### Frontier Detection 13 | The frontier detection module is adapted from Hasauino's [rrt_exploration](https://github.com/hasauino/rrt_exploration) (sincere thanks for his work), with series of improvements, including: 14 | 15 | - Refactored the entire code for clearer logic and efficient implementation; 16 | 17 | - Set sampling boundary automatically; 18 | 19 | - Dynamically adjust the number of nodes in the global tree. 20 | 21 | ### Hierarchical Planning 22 | 23 | The hierarchical exploration planning module is the core component of this framework. 24 | 25 | In the first phase, the planning space is divided into several subregions. After that, subregions with frontier points inside or mostly unknown are selected. The selected subregions are then sorted to provide global guidance for exploration. 26 | 27 | In the second phase, the subregions are visited one by one in the order determined in the first phase. In each subregion, various indicators, including global compatibility, information gain, and motion consistency, are calculated. Robot selects the exploration target with the highest comprehensive revenue. 28 | 29 | ![rqt_graph](./others/rosgraph.png) 30 | 31 | ## Quick Start 32 | 33 | ### Prerequisites 34 | 35 | We tried to minimize the unnecessary use of third-party libraries (like OpenCV, tf, etc). Simple external functions were implemented manually. 36 | 37 | - **ROS Environment** 38 | 39 | Make sure you have ROS environment properly installed. This project has been tested on several devices under [ROS-Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) and [ROS-Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu). (`Desktop-Full` version is recommended) 40 | 41 | - **Local Planner** 42 | 43 | Currently, TDLE uses `TEB` as its local planner by default. Install it with: 44 | 45 | sudo apt install ros-noetic-navigation 46 | sudo apt install ros-noetic-teb-local-planner 47 | 48 | At the same time, `DWA`, which is already included in `ros-noetic-navigation`, can also be used as the local planner. You can change the `planner_name` parameter in `tdle_sim.launch`. 49 | 50 | - **Cartographer_ROS** 51 | 52 | TDLE uses cartographer as its mapping module, with a few adaptions that make it suitable for exploration. Using [this fork](https://github.com/SeanZsya/cartographer_ros) instead of the original repository. 53 | 54 | In fact, TDLE supports any method that can generate an occupancy grid map with the message type `nav_msgs/OccupancyGrid`. 55 | 56 | - **Gazebo Models** (optional) 57 | 58 | The model files required by `robot(turtlebot3-waffle)` and `simulation scene(museum)` can be found in [gz_sim_models](https://github.com/SeanZsya/gz_sim_models), which is organized as a catkin package. You can clone it into your workspace and build it with `catkin_make`. 59 | 60 | If you want to use the `simulation scene(library)`, please download [extra models](https://github.com/aws-robotics/aws-robomaker-bookstore-world/tree/ros1/models), put them in your Gazebo model folder (usually `~/.gazebo/models`), and change the `world_name` parameter in `tdle_sim.launch`. 61 | 62 | 63 | ### Run TDLE 64 | 65 | - **For Gazebo Simulation** 66 | 67 | roslaunch tdle tdle_sim.launch 68 | 69 | - **For Field Test** 70 | 71 | roslaunch tdle tdle_real.launch 72 | 73 | 74 | ## Known Issues 75 | -------------------------------------------------------------------------------- /src/common_func.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | //euclideanDistance function: 4 | //return the Euclidean distance between 2 points 5 | float euclideanDistance(vector x, vector y) { 6 | float sum = 0.0; 7 | for (int i = 0; i < x.size(); i++) { 8 | sum += pow(x[i] - y[i], 2); 9 | } 10 | return sqrt(sum); 11 | } 12 | 13 | 14 | //sign function 15 | float sign(float n) 16 | { 17 | if (n<0.0){return -1.0;} 18 | else{return 1.0;} 19 | } 20 | 21 | 22 | //Nearest function: 23 | //find the nearest vertex from x_rand 24 | std::vector Nearest(std::vector > X,std::vector x_rand) 25 | { 26 | float min=euclideanDistance(X[0],x_rand); 27 | int indx=0; 28 | for (int c=1;c Steer(std::vector x_nearest,std::vector x_rand,float stride) 44 | { 45 | std::vector x_new; 46 | float norm=euclideanDistance(x_nearest,x_rand); 47 | x_new.push_back( x_nearest[0]+(stride/(norm+1e-4))*(x_rand[0]-x_nearest[0]) ); 48 | x_new.push_back( x_nearest[1]+(stride/(norm+1e-4))*(x_rand[1]-x_nearest[1]) ); 49 | return x_new; 50 | } 51 | 52 | //gridValue function: 53 | //returns grid value at location Xp 54 | //map data: 100 occupied -1 unknown 0 free 55 | int gridValue(nav_msgs::OccupancyGrid &mapData,std::vector Xp) 56 | { 57 | 58 | float resolution=mapData.info.resolution; 59 | float Xstartx=mapData.info.origin.position.x; 60 | float Xstarty=mapData.info.origin.position.y; 61 | int out; 62 | 63 | float width=mapData.info.width; 64 | std::vector Data=mapData.data; 65 | // ROS_WARN_THROTTLE(0.5,"Xp[0]: %f; Xp[1]: %f",Xp[0],Xp[1]); 66 | float indx=( floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) ); 67 | // ROS_WARN_THROTTLE(0.5,"indx: %f; mapsize:%d",indx,Data.size()); 68 | out=Data[int(indx)]; 69 | return out; 70 | } 71 | 72 | // ObstacleFree function 73 | //return value: -1: unknown 0: obstacle 1: free 74 | int ObstacleFree(std::vector xnear, std::vector &xnew, nav_msgs::OccupancyGrid mapsub) 75 | { 76 | float rez=float(mapsub.info.resolution)*.2; 77 | float stepz=int(ceil(euclideanDistance(xnew,xnear))/rez); 78 | std::vector xi=xnear; 79 | int obs=0;//obstacle 80 | int unk=0;//unknown 81 | geometry_msgs::Point p; 82 | for (int c=0;c= 50){ obs=1; } 86 | if (gridValue(mapsub,xi) ==-1){ unk=1; break;} 87 | } 88 | int out=0; 89 | xnew=xi; 90 | if (unk==1){ out=-1;} 91 | 92 | if (obs==1){ out=0;} 93 | 94 | if (obs!=1 && unk!=1){ out=1;} 95 | 96 | return out; 97 | } 98 | int ObstacleFree(geometry_msgs::Point point1, geometry_msgs::Point point2, nav_msgs::OccupancyGrid mapsub) 99 | { 100 | 101 | } 102 | 103 | //Initialize visualization_msgs::Marker 104 | void visMarkerInit(visualization_msgs::Marker &marker, std::string ID, 105 | int type, int action, float scale, float r, float g, float b, float a) 106 | { 107 | marker.header.frame_id= ID; 108 | marker.type = type; // 8:points, 5:line_list, 6:cube_list 109 | marker.action = action;// 0: add, 2: delete 110 | marker.scale.x = scale; 111 | marker.scale.y = scale; 112 | marker.scale.z = scale; 113 | marker.color.r = r; 114 | marker.color.g = g; 115 | marker.color.b = b; 116 | marker.color.a = a; 117 | } 118 | 119 | void visMarkerSet (visualization_msgs::Marker &marker, std::vector &point) 120 | { 121 | geometry_msgs::Point p; 122 | p.x=point[0]; 123 | p.y=point[1]; 124 | p.z=0.0; 125 | marker.points.push_back(p); 126 | } -------------------------------------------------------------------------------- /launch/include/move_base_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | 14 | 15 | 18 | 19 | 20 | 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 | -------------------------------------------------------------------------------- /src/fpoint_detector_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace std; 5 | 6 | nav_msgs::Odometry odom; 7 | nav_msgs::OccupancyGrid mapData; 8 | geometry_msgs::Point map_origin; 9 | float map_width, map_height; 10 | vector x_bot; 11 | geometry_msgs::PointStamped detected_point; 12 | visualization_msgs::Marker grid_vis; 13 | float stride; 14 | 15 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) 16 | { 17 | mapData = *msg; 18 | map_width = mapData.info.width * mapData.info.resolution; 19 | map_height = mapData.info.height * mapData.info.resolution; 20 | map_origin = mapData.info.origin.position; 21 | } 22 | 23 | void odomCallBack(const nav_msgs::Odometry& msg) 24 | { 25 | odom = msg; 26 | x_bot.clear(); 27 | x_bot.push_back(odom.pose.pose.position.x); 28 | x_bot.push_back(odom.pose.pose.position.y); 29 | } 30 | 31 | int rrtExpand (vector< vector > &tree_nodes, visualization_msgs::Marker &vis_marker) 32 | { 33 | vector x_rand,x_nearest,x_new,x_start; 34 | float xr,yr; 35 | // Sampling random point 36 | // Mersenne Twister generator, faster than rand() 37 | random_device rd; 38 | mt19937 gen(rd()); 39 | uniform_real_distribution<> dis(0, 1); 40 | xr = dis(gen) * map_width + map_origin.x; 41 | yr = dis(gen) * map_height + map_origin.y; 42 | x_rand.clear(); 43 | x_rand.push_back(xr); 44 | x_rand.push_back(yr); 45 | x_nearest=Nearest(tree_nodes,x_rand); 46 | x_new=Steer(x_nearest,x_rand,stride); 47 | // Check new point 48 | // ObstacleFree 1:free -1:unkown (frontier region) 0:obstacle 49 | int det_check=ObstacleFree(x_nearest,x_new,mapData); 50 | if (det_check==-1) 51 | { 52 | detected_point.header.stamp=ros::Time(0); 53 | detected_point.header.frame_id=mapData.header.frame_id; 54 | detected_point.point.x=x_new[0]; 55 | detected_point.point.y=x_new[1]; 56 | detected_point.point.z=0.0; 57 | // visMarkerSet(fpoint_g_vis,x_new); 58 | } 59 | else if (det_check==1) 60 | { 61 | tree_nodes.push_back(x_new); 62 | // keep enough points, dicard the rest 63 | if (tree_nodes.size()> (2*map_height*map_width/stride/stride)) 64 | tree_nodes.erase(tree_nodes.begin()); 65 | // for visualization only 66 | if (vis_marker.points.size()> (2*map_height*map_width/stride/stride)) 67 | vis_marker.points.erase(vis_marker.points.begin(),vis_marker.points.begin()+2); 68 | visMarkerSet(vis_marker,x_new); 69 | visMarkerSet(vis_marker,x_nearest); 70 | } 71 | return det_check; 72 | } 73 | 74 | int main(int argc, char **argv) 75 | { 76 | ros::init(argc, argv, "fpoint_detector"); 77 | ros::NodeHandle nh; 78 | 79 | // fetching all parameters 80 | vector< std::vector > V_g, V_l; 81 | visualization_msgs::Marker fpoint_g_vis, tree_g_vis, tree_l_vis; 82 | 83 | ros::param::param("/stride", stride, 0.5); 84 | 85 | string map_topic, odom_topic; 86 | ros::param::param("/map_topic", map_topic, "/map"); 87 | ros::param::param("/odom_topic", odom_topic, "/odom"); 88 | ros::Subscriber mapsub= nh.subscribe(map_topic, 1, mapCallBack); 89 | ros::Subscriber odomsub= nh.subscribe(odom_topic, 1, odomCallBack); 90 | ros::Publisher grid_pub = nh.advertise("/grid_marker", 1); 91 | ros::Publisher targetspub = nh.advertise("/detected_points", 10); 92 | ros::Publisher localmkpub = nh.advertise("local_shapes", 10); 93 | ros::Publisher globalmkpub = nh.advertise("global_shapes", 10); 94 | ros::Publisher testpub = nh.advertise("test_shapes", 10); 95 | ros::Rate rate(150); 96 | 97 | // wait until map is received 98 | while (mapData.header.seq<1 or mapData.data.size()<100) {ros::spinOnce(); rate.sleep();} 99 | 100 | visMarkerInit(tree_g_vis, mapData.header.frame_id, 5, 0, 0.03, 0, 0.5, 0.5, 0.15); 101 | visMarkerInit(tree_l_vis, mapData.header.frame_id, 5, 0, 0.03, 1, 0.1, 0.2, 0.2); 102 | 103 | vector x_start; 104 | x_start.push_back(0.0); x_start.push_back(0.0); 105 | V_g.push_back(x_start); 106 | V_l.push_back(x_start); 107 | 108 | // Main loop 109 | while (ros::ok()) 110 | { 111 | // Local Tree 112 | int det_check = rrtExpand(V_l,tree_l_vis); 113 | if(det_check == -1) //once detected, publish and reset 114 | { 115 | targetspub.publish(detected_point); 116 | V_l.clear(); 117 | V_l.push_back(x_bot); 118 | tree_l_vis.points.clear(); 119 | } 120 | 121 | // Global Tree 122 | rrtExpand(V_g,tree_g_vis); 123 | targetspub.publish(detected_point); 124 | 125 | //Visualization 126 | globalmkpub.publish(tree_g_vis); 127 | localmkpub.publish(tree_l_vis); 128 | 129 | ros::spinOnce(); 130 | rate.sleep(); 131 | } 132 | return 0; 133 | } -------------------------------------------------------------------------------- /src/fpoint_filter_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace std; 5 | 6 | vector frontier_points; 7 | nav_msgs::OccupancyGrid mapData, costmapData; 8 | geometry_msgs::Point map_origin, next_center; 9 | float map_width, map_height; 10 | 11 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) 12 | { 13 | mapData = *msg; 14 | map_width = msg->info.width * msg->info.resolution; 15 | map_height = msg->info.height * msg->info.resolution; 16 | map_origin = msg->info.origin.position; 17 | } 18 | 19 | void costmapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) 20 | { 21 | costmapData = *msg; 22 | } 23 | 24 | void fpointCallBack(const geometry_msgs::PointStamped& msg) 25 | { 26 | // Only keep 200 fpoints 27 | if (frontier_points.size() > 200) 28 | frontier_points.erase(frontier_points.begin()); 29 | 30 | // Deploy frontier points filter (MeanShift of disFilter) 31 | // frontier_points = meanshiftFilter(msg.point, frontier_points, 0.3, 0.1); 32 | frontier_points = disFilter(msg.point, frontier_points); 33 | } 34 | 35 | vector disFilter(geometry_msgs::Point point, vector& fpoints) 36 | { 37 | //add new point if not close to points in fpoints 38 | bool is_close = false; 39 | for (int i = 0; i < fpoints.size(); i++) { 40 | if (sqrt(pow(point.x - fpoints[i].x, 2) + pow(point.y - fpoints[i].y, 2)) < 0.5) { 41 | is_close = true; 42 | break; 43 | } 44 | } 45 | if (!is_close) fpoints.push_back(point); 46 | 47 | return fpoints; 48 | } 49 | 50 | 51 | // vector meanshiftFilter(geometry_msgs::Point point, vector &fpoints, float kernel_bandwidth, float convergence_threshold) { 52 | // fpoints.push_back(point); 53 | // vector centroids; 54 | // for (int i = 0; i < fpoints.size(); i++) { 55 | // geometry_msgs::Point centroid = fpoints[i]; 56 | // geometry_msgs::Point next_centroid; 57 | // while (1) { 58 | // float sum_x = 0.0; 59 | // float sum_y = 0.0; 60 | // float sum_w = 0.0; 61 | // for (int j = 0; j < fpoints.size(); j++) { 62 | // float dist = sqrt(pow(centroid.x - fpoints[j].x, 2) + pow(centroid.y - fpoints[j].y, 2)); 63 | // float weight = exp(-dist * dist / (2 * kernel_bandwidth * kernel_bandwidth)); 64 | // sum_x += weight * fpoints[j].x; 65 | // sum_y += weight * fpoints[j].y; 66 | // sum_w += weight; 67 | // } 68 | // next_centroid.x = sum_x / sum_w; 69 | // next_centroid.y = sum_y / sum_w; 70 | // if (sqrt(pow(centroid.x - next_centroid.x, 2) + pow(centroid.y - next_centroid.y, 2)) < convergence_threshold) { 71 | // break; 72 | // } 73 | // centroid = next_centroid; 74 | // } 75 | // centroids.push_back(centroid); 76 | // } 77 | 78 | // return centroids; 79 | // } 80 | 81 | tdle::PointArray delPrevious(vector& filtered_points, visualization_msgs::Marker& fpoint_vis) 82 | { 83 | tdle::PointArray candi_points; 84 | fpoint_vis.points.clear(); 85 | for (int i = 0; i < filtered_points.size(); i++) { 86 | geometry_msgs::Point point = filtered_points[i]; 87 | int grid_x = (point.x - map_origin.x) / mapData.info.resolution; 88 | int grid_y = (point.y - map_origin.y) / mapData.info.resolution; 89 | 90 | //filter out point with costmap value more than threshold 91 | int costmap_value = costmapData.data[grid_y * costmapData.info.width + grid_x]; 92 | if (costmap_value > 30) { 93 | filtered_points.erase(filtered_points.begin() + i); 94 | i--; 95 | continue; 96 | } 97 | 98 | // filter out point with few unknown grid in circle 99 | int unknown_num = 0; 100 | for (int j = -2; j <= 2; j++) 101 | for (int k = -2; k <= 2; k++) 102 | if (mapData.data[(grid_y + 2 * j) * mapData.info.width + grid_x + 2 * k] == -1) unknown_num++; 103 | if (unknown_num < 5){ 104 | filtered_points.erase(filtered_points.begin() + i); 105 | i--; 106 | continue; 107 | } 108 | 109 | candi_points.points.push_back(point); 110 | fpoint_vis.points.push_back(point); 111 | } 112 | return candi_points; 113 | } 114 | 115 | int main(int argc, char **argv) 116 | { 117 | ros::init(argc, argv, "fpoint_filter"); 118 | ros::NodeHandle nh; 119 | 120 | ros::Subscriber mapsub = nh.subscribe("/map", 10, mapCallBack); 121 | ros::Subscriber costmap_sub = nh.subscribe("/move_base/global_costmap/costmap", 1, costmapCallBack); 122 | ros::Subscriber fpoint_sub = nh.subscribe("/detected_points", 1, fpointCallBack); 123 | ros::Publisher fpoint_pub = nh.advertise("/filtered_points", 10); 124 | ros::Publisher fpoint_vis_pub = nh.advertise("/candidate_points", 10); 125 | ros::Rate rate(10); 126 | 127 | visualization_msgs::Marker fpoint_vis; 128 | 129 | float bandwidth = 0.3; 130 | float conv_thr = 0.1; 131 | 132 | // wait until map is received 133 | while (mapData.header.seq<1 or mapData.data.size()<100) 134 | { 135 | ROS_WARN_THROTTLE(1,"Waiting for the map"); 136 | ros::spinOnce(); rate.sleep(); 137 | } 138 | while (costmapData.data.size()<100) 139 | { 140 | ROS_WARN_THROTTLE(1,"Waiting for the global cost map"); 141 | ros::spinOnce(); rate.sleep(); 142 | } 143 | 144 | visMarkerInit(fpoint_vis, mapData.header.frame_id, 8, 0, 0.2, 29.0/255.0, 111.0/255.0, 210.0/255.0, 0.5); 145 | 146 | while (ros::ok()) 147 | { 148 | tdle::PointArray candi_points = delPrevious(frontier_points, fpoint_vis); 149 | 150 | fpoint_pub.publish(candi_points); 151 | fpoint_vis_pub.publish(fpoint_vis); 152 | 153 | ros::spinOnce(); 154 | rate.sleep(); 155 | } 156 | 157 | return 0; 158 | } -------------------------------------------------------------------------------- /src/fpoint_selection_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | nav_msgs::Odometry odom; 10 | nav_msgs::OccupancyGrid mapData; 11 | EXP_STATE state = NORMAL; 12 | geometry_msgs::Point map_origin, next_center; 13 | float map_width, map_height; 14 | vector filtered_points, candidate_fpoints, waypoints; 15 | 16 | 17 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) 18 | { 19 | mapData = *msg; 20 | map_width = msg->info.width * msg->info.resolution; 21 | map_height = msg->info.height * msg->info.resolution; 22 | map_origin = msg->info.origin.position; 23 | } 24 | 25 | void odomCallBack(const nav_msgs::Odometry& msg) 26 | { 27 | odom = msg; 28 | } 29 | 30 | void fpCallBack(const tdle::PointArray& msg) 31 | { 32 | filtered_points = msg.points; 33 | } 34 | 35 | void cand_fpCallBack(const tdle::PointArray& msg) 36 | { 37 | next_center = msg.points.back(); 38 | candidate_fpoints = msg.points; 39 | candidate_fpoints.pop_back(); 40 | } 41 | 42 | // global compatibility 43 | float gloComp(geometry_msgs::Point fpoint,geometry_msgs::Point next_center) 44 | { 45 | float cost = max(abs(fpoint.x - next_center.x),abs(fpoint.y - next_center.y))-max(map_height,map_width)/6; 46 | return cost; 47 | } 48 | 49 | // motion consistency 50 | float motionCosist(geometry_msgs::Point fpoint) 51 | { 52 | geometry_msgs::Point r_position = odom.pose.pose.position; 53 | geometry_msgs::Quaternion r_orient = odom.pose.pose.orientation; 54 | float angle = atan2(fpoint.y - r_position.y, fpoint.x - r_position.x); 55 | float yaw = atan2( 56 | 2 *(r_orient.w * r_orient.z + r_orient.x * r_orient.y), 57 | 1 - 2 * (r_orient.y * r_orient.y + r_orient.z * r_orient.z) ); 58 | float cost = exp(2 * ( 0.637 * abs(angle - yaw) - 1)); 59 | // ROS_INFO_THROTTLE(3,"motion consistency: %f",cost); 60 | return cost; 61 | } 62 | 63 | // info gain: fpoints can be seen in filtered_points at candidate point 64 | float infoGain(geometry_msgs::Point fpoint) 65 | { 66 | float cost = 0; 67 | for(int i=0; i zscoreNor(vector data) 76 | { 77 | float sum = 0; 78 | for(int i=0; i norm_data; 86 | for(int i=0; i gloComp_vec, motionCosist_vec, infoGain_vec; 102 | for (const auto& point : candidate_fpoints) 103 | { 104 | gloComp_vec.push_back(gloComp(point, next_center)); 105 | motionCosist_vec.push_back(motionCosist(point)); 106 | infoGain_vec.push_back(infoGain(point)); 107 | } 108 | 109 | vector norm_gloComp = zscoreNor(gloComp_vec); 110 | vector norm_infoGain = zscoreNor(infoGain_vec); 111 | 112 | for (int i = 0; i < candidate_fpoints.size(); i++) 113 | { 114 | float score = 0.5 * norm_gloComp[i] + 0.5 * norm_infoGain[i] - motionCosist_vec[i]; 115 | if (score > best_score) 116 | { 117 | best_score = score; 118 | target_point = candidate_fpoints[i]; 119 | } 120 | } 121 | 122 | return target_point; 123 | } 124 | 125 | void stateCheck() 126 | { 127 | // Only works with DWA Local Planner currently (TEB Local Planner not supported) 128 | waypoints.push_back(odom.pose.pose.position); 129 | int count = 0; 130 | ROS_WARN_THROTTLE(3,"check state"); 131 | for(int i=0; i15 && count > 5) 138 | { 139 | if(candidate_fpoints.size() == 0) 140 | { 141 | ROS_WARN_THROTTLE(3,"No candidate fpoint, return to origin"); 142 | state = FINISHED; 143 | } 144 | else 145 | { 146 | for(int i=0; i("/map_topic", map_topic, "/map"); 169 | ros::param::param("/odom_topic", odom_topic, "/odom"); 170 | ros::param::param("/local_plan", local_plan, "/move_base/TebLocalPlannerROS/local_plan"); 171 | ros::Subscriber mapsub = nh.subscribe(map_topic, 10, mapCallBack); 172 | ros::Subscriber odomsub = nh.subscribe(odom_topic, 10, odomCallBack); 173 | ros::Subscriber fpoint_sub = nh.subscribe("/filtered_points", 10, fpCallBack); 174 | ros::Subscriber cand_fp_sub = nh.subscribe("/candidate_fpoint", 10, cand_fpCallBack); 175 | // ros::Timer state_timer = nh.createTimer(ros::Duration(0.5), boost::bind(stateCheck)); 176 | 177 | actionlib::SimpleActionClient ac("move_base", true); 178 | 179 | ros::Rate rate(10); 180 | 181 | //wait for the action server to come up 182 | while(!ac.waitForServer(ros::Duration(5.0))){ROS_WARN("Waiting move_base action server");} 183 | 184 | // wait until map is received 185 | while (mapData.header.seq<1 or mapData.data.size()<100) {ros::spinOnce(); rate.sleep();} 186 | 187 | // Main loop 188 | while (ros::ok()) 189 | { 190 | geometry_msgs::Point target_point; 191 | if(state == FINISHED) 192 | target_point = waypoints[0]; 193 | else if(state == STUCKED) 194 | target_point = waypoints[waypoints.size()-15]; 195 | else 196 | target_point = getTarget(); 197 | 198 | move_base_msgs::MoveBaseGoal goal; 199 | goal.target_pose.header.frame_id = "map"; 200 | goal.target_pose.header.stamp = ros::Time::now(); 201 | goal.target_pose.pose.position.x = target_point.x; 202 | goal.target_pose.pose.position.y = target_point.y; 203 | goal.target_pose.pose.orientation.w = 1.0; 204 | 205 | ac.sendGoal(goal); 206 | 207 | // ROS_WARN("heading to subregion %d", goal.target_pose); 208 | 209 | ros::spinOnce(); 210 | ros::Duration(0.7).sleep(); 211 | } 212 | 213 | return 0; 214 | } -------------------------------------------------------------------------------- /launch/include/move_base_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 15 | 18 | 19 | 20 | 55 | 56 | 57 | 58 | 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 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | -------------------------------------------------------------------------------- /param/rviz_config/single.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 104 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /Odometry1/Shape1 9 | - /Odometry2/Shape1 10 | Splitter Ratio: 0.38461539149284363 11 | Tree Height: 685 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.20000000298023224 37 | Cell Size: 0.5 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 1000 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/Polygon 56 | Color: 0; 0; 255 57 | Enabled: false 58 | Name: Polygon 59 | Queue Size: 10 60 | Topic: /move_base_node/local_costmap/footprint 61 | Unreliable: false 62 | Value: false 63 | - Class: rviz/TF 64 | Enabled: false 65 | Filter (blacklist): "" 66 | Filter (whitelist): "" 67 | Frame Timeout: 15 68 | Frames: 69 | All Enabled: false 70 | Marker Alpha: 1 71 | Marker Scale: 1 72 | Name: TF 73 | Show Arrows: false 74 | Show Axes: true 75 | Show Names: false 76 | Tree: 77 | {} 78 | Update Interval: 0 79 | Value: false 80 | - Alpha: 1 81 | Class: rviz/Map 82 | Color Scheme: map 83 | Draw Behind: true 84 | Enabled: true 85 | Name: Map 86 | Topic: /map 87 | Unreliable: false 88 | Use Timestamp: false 89 | Value: true 90 | - Alpha: 0.10000000149011612 91 | Class: rviz/Map 92 | Color Scheme: costmap 93 | Draw Behind: true 94 | Enabled: false 95 | Name: Map 96 | Topic: /move_base/global_costmap/costmap 97 | Unreliable: false 98 | Use Timestamp: false 99 | Value: false 100 | - Alpha: 0.5 101 | Class: rviz/Map 102 | Color Scheme: costmap 103 | Draw Behind: true 104 | Enabled: false 105 | Name: Map 106 | Topic: /move_base/local_costmap/costmap 107 | Unreliable: false 108 | Use Timestamp: false 109 | Value: false 110 | - Alpha: 1 111 | Buffer Length: 1 112 | Class: rviz/Path 113 | Color: 25; 255; 0 114 | Enabled: true 115 | Head Diameter: 0.30000001192092896 116 | Head Length: 0.20000000298023224 117 | Length: 0.30000001192092896 118 | Line Style: Lines 119 | Line Width: 0.029999999329447746 120 | Name: Path 121 | Offset: 122 | X: 0 123 | Y: 0 124 | Z: 0 125 | Pose Color: 255; 85; 255 126 | Pose Style: None 127 | Queue Size: 10 128 | Radius: 0.029999999329447746 129 | Shaft Diameter: 0.10000000149011612 130 | Shaft Length: 0.10000000149011612 131 | Topic: /move_base/TebLocalPlannerROS/global_plan 132 | Unreliable: false 133 | Value: true 134 | - Alpha: 1 135 | Buffer Length: 1 136 | Class: rviz/Path 137 | Color: 25; 255; 0 138 | Enabled: false 139 | Head Diameter: 0.30000001192092896 140 | Head Length: 0.20000000298023224 141 | Length: 0.30000001192092896 142 | Line Style: Lines 143 | Line Width: 0.029999999329447746 144 | Name: Path 145 | Offset: 146 | X: 0 147 | Y: 0 148 | Z: 0 149 | Pose Color: 255; 85; 255 150 | Pose Style: None 151 | Queue Size: 10 152 | Radius: 0.029999999329447746 153 | Shaft Diameter: 0.10000000149011612 154 | Shaft Length: 0.10000000149011612 155 | Topic: /move_base/NavfnROS/plan 156 | Unreliable: false 157 | Value: false 158 | - Alpha: 1 159 | Buffer Length: 1 160 | Class: rviz/Path 161 | Color: 32; 74; 135 162 | Enabled: false 163 | Head Diameter: 0.30000001192092896 164 | Head Length: 0.20000000298023224 165 | Length: 0.30000001192092896 166 | Line Style: Lines 167 | Line Width: 0.029999999329447746 168 | Name: Path 169 | Offset: 170 | X: 0 171 | Y: 0 172 | Z: 0 173 | Pose Color: 255; 85; 255 174 | Pose Style: None 175 | Queue Size: 10 176 | Radius: 0.029999999329447746 177 | Shaft Diameter: 0.10000000149011612 178 | Shaft Length: 0.10000000149011612 179 | Topic: /move_base/DWAPlannerROS/global_plan 180 | Unreliable: false 181 | Value: false 182 | - Alpha: 1 183 | Autocompute Intensity Bounds: true 184 | Autocompute Value Bounds: 185 | Max Value: 10 186 | Min Value: -10 187 | Value: true 188 | Axis: Z 189 | Channel Name: intensity 190 | Class: rviz/LaserScan 191 | Color: 255; 255; 255 192 | Color Transformer: Intensity 193 | Decay Time: 0 194 | Enabled: true 195 | Invert Rainbow: false 196 | Max Color: 255; 255; 255 197 | Min Color: 0; 0; 0 198 | Name: LaserScan 199 | Position Transformer: XYZ 200 | Queue Size: 10 201 | Selectable: true 202 | Size (Pixels): 3 203 | Size (m): 0.009999999776482582 204 | Style: Flat Squares 205 | Topic: /base_scan 206 | Unreliable: false 207 | Use Fixed Frame: true 208 | Use rainbow: true 209 | Value: true 210 | - Class: rviz/Marker 211 | Enabled: true 212 | Marker Topic: /global_shapes 213 | Name: Marker 214 | Namespaces: 215 | "": true 216 | Queue Size: 100 217 | Value: true 218 | - Class: rviz/Marker 219 | Enabled: true 220 | Marker Topic: /local_shapes 221 | Name: Marker 222 | Namespaces: 223 | "": true 224 | Queue Size: 100 225 | Value: true 226 | - Class: rviz/Marker 227 | Enabled: true 228 | Marker Topic: /centroids 229 | Name: Marker 230 | Namespaces: 231 | {} 232 | Queue Size: 100 233 | Value: true 234 | - Class: rviz/Marker 235 | Enabled: true 236 | Marker Topic: /grid_marker 237 | Name: Marker 238 | Namespaces: 239 | grid: true 240 | Queue Size: 100 241 | Value: true 242 | - Class: rviz/Marker 243 | Enabled: true 244 | Marker Topic: /candidate_points 245 | Name: Marker 246 | Namespaces: 247 | "": true 248 | Queue Size: 100 249 | Value: true 250 | - Class: rviz/Marker 251 | Enabled: true 252 | Marker Topic: /subregion_marker 253 | Name: Marker 254 | Namespaces: 255 | SubRegion: true 256 | Queue Size: 100 257 | Value: true 258 | - Angle Tolerance: 0.10000000149011612 259 | Class: rviz/Odometry 260 | Covariance: 261 | Orientation: 262 | Alpha: 0.5 263 | Color: 255; 255; 127 264 | Color Style: Unique 265 | Frame: Local 266 | Offset: 1 267 | Scale: 1 268 | Value: true 269 | Position: 270 | Alpha: 0.30000001192092896 271 | Color: 204; 51; 204 272 | Scale: 1 273 | Value: true 274 | Value: false 275 | Enabled: false 276 | Keep: 999999999 277 | Name: Odometry 278 | Position Tolerance: 0.10000000149011612 279 | Queue Size: 10 280 | Shape: 281 | Alpha: 1 282 | Axes Length: 1 283 | Axes Radius: 0.10000000149011612 284 | Color: 78; 154; 6 285 | Head Length: 0 286 | Head Radius: 0 287 | Shaft Length: 0.15000000596046448 288 | Shaft Radius: 0.03999999910593033 289 | Value: Arrow 290 | Topic: /odom 291 | Unreliable: false 292 | Value: false 293 | - Alpha: 1 294 | Class: rviz/PointStamped 295 | Color: 204; 41; 204 296 | Enabled: false 297 | History Length: 1 298 | Name: PointStamped 299 | Queue Size: 10 300 | Radius: 0.20000000298023224 301 | Topic: /detected_points 302 | Unreliable: false 303 | Value: false 304 | - Class: rviz/MarkerArray 305 | Enabled: true 306 | Marker Topic: /trajectory_node_list 307 | Name: MarkerArray 308 | Namespaces: 309 | Trajectory 0: true 310 | Queue Size: 100 311 | Value: true 312 | - Angle Tolerance: 0.10000000149011612 313 | Class: rviz/Odometry 314 | Covariance: 315 | Orientation: 316 | Alpha: 0.5 317 | Color: 255; 255; 127 318 | Color Style: Unique 319 | Frame: Local 320 | Offset: 1 321 | Scale: 1 322 | Value: true 323 | Position: 324 | Alpha: 0.30000001192092896 325 | Color: 204; 51; 204 326 | Scale: 1 327 | Value: true 328 | Value: false 329 | Enabled: false 330 | Keep: 100 331 | Name: Odometry 332 | Position Tolerance: 0.10000000149011612 333 | Queue Size: 10 334 | Shape: 335 | Alpha: 0.5 336 | Axes Length: 1 337 | Axes Radius: 0.10000000149011612 338 | Color: 255; 25; 0 339 | Head Length: 0.10000000149011612 340 | Head Radius: 0.10000000149011612 341 | Shaft Length: 0.20000000298023224 342 | Shaft Radius: 0.05000000074505806 343 | Value: Arrow 344 | Topic: /odom 345 | Unreliable: false 346 | Value: false 347 | - Class: rviz/Marker 348 | Enabled: false 349 | Marker Topic: /exploration_polygon_marker 350 | Name: Marker 351 | Namespaces: 352 | {} 353 | Queue Size: 100 354 | Value: false 355 | - Class: rviz/MarkerArray 356 | Enabled: true 357 | Marker Topic: /explore/frontiers 358 | Name: MarkerArray 359 | Namespaces: 360 | {} 361 | Queue Size: 100 362 | Value: true 363 | - Alpha: 1 364 | Class: rviz/RobotModel 365 | Collision Enabled: false 366 | Enabled: true 367 | Links: 368 | All Links Enabled: true 369 | Expand Joint Details: false 370 | Expand Link Details: false 371 | Expand Tree: false 372 | Link Tree Style: Links in Alphabetic Order 373 | base_footprint: 374 | Alpha: 1 375 | Show Axes: false 376 | Show Trail: false 377 | base_link: 378 | Alpha: 1 379 | Show Axes: false 380 | Show Trail: false 381 | Value: true 382 | base_scan: 383 | Alpha: 1 384 | Show Axes: false 385 | Show Trail: false 386 | Value: true 387 | caster_back_left_link: 388 | Alpha: 1 389 | Show Axes: false 390 | Show Trail: false 391 | Value: true 392 | caster_back_right_link: 393 | Alpha: 1 394 | Show Axes: false 395 | Show Trail: false 396 | Value: true 397 | imu_link: 398 | Alpha: 1 399 | Show Axes: false 400 | Show Trail: false 401 | wheel_left_link: 402 | Alpha: 1 403 | Show Axes: false 404 | Show Trail: false 405 | Value: true 406 | wheel_right_link: 407 | Alpha: 1 408 | Show Axes: false 409 | Show Trail: false 410 | Value: true 411 | Name: RobotModel 412 | Robot Description: robot_description 413 | TF Prefix: "" 414 | Update Interval: 0 415 | Value: true 416 | Visual Enabled: true 417 | Enabled: true 418 | Global Options: 419 | Background Color: 238; 238; 236 420 | Default Light: true 421 | Fixed Frame: map 422 | Frame Rate: 30 423 | Name: root 424 | Tools: 425 | - Class: rviz/Interact 426 | Hide Inactive Objects: true 427 | - Class: rviz/SetGoal 428 | Topic: /move_base_simple/goal 429 | - Class: rviz/PublishPoint 430 | Single click: true 431 | Topic: /clicked_point 432 | - Class: rviz/Measure 433 | Value: true 434 | Views: 435 | Current: 436 | Class: rviz/Orbit 437 | Distance: 23.41075325012207 438 | Enable Stereo Rendering: 439 | Stereo Eye Separation: 0.05999999865889549 440 | Stereo Focal Distance: 1 441 | Swap Stereo Eyes: false 442 | Value: false 443 | Field of View: 0.7853981852531433 444 | Focal Point: 445 | X: 1.7360970973968506 446 | Y: -2.7781388759613037 447 | Z: 3.7776453495025635 448 | Focal Shape Fixed Size: true 449 | Focal Shape Size: 0.05000000074505806 450 | Invert Z Axis: false 451 | Name: Current View 452 | Near Clip Distance: 0.009999999776482582 453 | Pitch: 1.5697963237762451 454 | Target Frame: 455 | Yaw: 3.1453747749328613 456 | Saved: ~ 457 | Window Geometry: 458 | Displays: 459 | collapsed: true 460 | Height: 1023 461 | Hide Left Dock: true 462 | Hide Right Dock: true 463 | QMainWindow State: 000000ff00000000fd0000000400000000000001a400000352fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000352000000c900fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001d1000000ad0000005c00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000004dfc0100000002fb0000000800540069006d00650100000000000004f3000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004f30000035200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 464 | Selection: 465 | collapsed: false 466 | Time: 467 | collapsed: false 468 | Tool Properties: 469 | collapsed: false 470 | Views: 471 | collapsed: true 472 | Width: 1267 473 | X: 68 474 | Y: 27 475 | -------------------------------------------------------------------------------- /src/subregion_arrangement_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | nav_msgs::Odometry odom; 8 | nav_msgs::OccupancyGrid mapData; 9 | geometry_msgs::Point map_origin; 10 | float map_width, map_height; 11 | int sr_bot; 12 | vector filtered_points, center_points; 13 | 14 | // CallBack Functions ======================================== 15 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) 16 | { 17 | mapData = *msg; 18 | map_width = msg->info.width * msg->info.resolution; 19 | map_height = msg->info.height * msg->info.resolution; 20 | map_origin = msg->info.origin.position; 21 | } 22 | 23 | void odomCallBack(const nav_msgs::Odometry& msg) 24 | { 25 | odom = msg; 26 | } 27 | 28 | void fpCallBack(const tdle::PointArray& msg) 29 | { 30 | filtered_points = msg.points; 31 | } 32 | 33 | // Visualization Functions ======================================== 34 | void drawSubRegi(vector sr_sorted, ros::Publisher marker_pub) 35 | { 36 | visualization_msgs::Marker marker; 37 | marker.header.frame_id = mapData.header.frame_id; 38 | marker.ns = "SubRegion"; 39 | marker.id = 0; 40 | marker.type = visualization_msgs::Marker::CUBE_LIST; 41 | marker.action = visualization_msgs::Marker::ADD; 42 | marker.pose.orientation.w = 1.0; 43 | // Set the scale of the marker. 44 | marker.scale.x = map_width / 3; 45 | marker.scale.y = map_height / 3; 46 | marker.scale.z = 0.01; 47 | marker.lifetime = ros::Duration(); 48 | 49 | // Calculate the grid positions. 50 | double grid_width = map_width / 3; 51 | double grid_height = map_height / 3; 52 | 53 | geometry_msgs::Point position; 54 | std_msgs::ColorRGBA color; 55 | 56 | for(int i=0; i selectSubRegi() 141 | { 142 | //select grids(1-9) with filtered point inside 143 | vector sr_sel; 144 | int num_subregion = 9; 145 | double grid_width = map_width / 3; 146 | double grid_height = map_height / 3; 147 | 148 | for(int i=0; i= 2) 164 | { 165 | fpoint_inside = true; 166 | break; 167 | } 168 | } 169 | //unknown or has filtered points inside, add to sr_sorted 170 | if(isUnknown(center) || fpoint_inside) 171 | sr_sel.push_back(i); 172 | 173 | //determine which subregion the robot is in here by the way 174 | if(isInside(center, odom.pose.pose.position)) 175 | sr_bot = i; 176 | 177 | //save center_points by the way 178 | center_points.push_back(center); 179 | } 180 | 181 | return sr_sel; 182 | } 183 | 184 | bool isInside(geometry_msgs::Point center, geometry_msgs::Point point) 185 | { 186 | double grid_width = map_width / 3; 187 | double grid_height = map_height / 3; 188 | double x = center.x; 189 | double y = center.y; 190 | double x1 = x - grid_width/2; 191 | double x2 = x + grid_width/2; 192 | double y1 = y - grid_height/2; 193 | double y2 = y + grid_height/2; 194 | if((point.x > x1) && (point.x < x2) && (point.y > y1) && (point.y < y2)) 195 | return true; 196 | else 197 | return false; 198 | } 199 | 200 | bool isUnknown(geometry_msgs::Point center) 201 | { 202 | double grid_width = map_width / 3; 203 | double grid_height = map_height / 3; 204 | double x = center.x; 205 | double y = center.y; 206 | double x1 = x - grid_width/2; 207 | double x2 = x + grid_width/2; 208 | double y1 = y - grid_height/2; 209 | double y2 = y + grid_height/2; 210 | double x_step = (x2 - x1) / 10; 211 | double y_step = (y2 - y1) / 10; 212 | int unknown_count = 0; 213 | for(int i=0; i<10; i++) 214 | { 215 | for(int j=0; j<10; j++) 216 | { 217 | float x = x1 + i * x_step; 218 | float y = y1 + j * y_step; 219 | vector grid; 220 | grid.push_back(x); 221 | grid.push_back(y); 222 | int gridvalue = gridValue(mapData, grid);// 223 | if(gridvalue == -1) 224 | unknown_count++; 225 | } 226 | } 227 | if(unknown_count > 80) 228 | return true; 229 | else 230 | return false; 231 | } 232 | 233 | // Sort SubRegions ============================================== 234 | float route_length(vector> full_graph, vector route) 235 | { 236 | // ROS_WARN("stage2"); 237 | float length = 0; 238 | for (int i = 0; i < route.size() - 1; i++) { 239 | length += full_graph[route[i]][route[i+1]]; 240 | } 241 | // ROS_WARN("stage3"); 242 | return length; 243 | } 244 | 245 | float navi_cost(geometry_msgs::Point next_center, geometry_msgs::Point fpoint) 246 | { 247 | // distance to next subregion center 248 | float cost = sqrt(pow(next_center.x - fpoint.x, 2) + pow(next_center.y - fpoint.y, 2)); 249 | // distance to next subregion edge 250 | // float cost = max(abs(next_center.x - fpoint.x),abs(next_center.y - fpoint.y)) - max(map_height,map_width)/6; 251 | return cost; 252 | } 253 | 254 | float similarity_dtw(vector> full_graph, vector sr_sel, vector sr_sorted_before) 255 | { 256 | int n = sr_sel.size(); 257 | int m = sr_sorted_before.size(); 258 | // Initialize the cost matrix with large values 259 | vector> cost(n, vector(m, 1e9)); 260 | // Initialize the first row and column with 0 261 | cost[0][0] = full_graph[sr_sel[0]][sr_sorted_before[0]]; 262 | for (int i = 1; i < n; i++) { 263 | cost[i][0] = cost[i-1][0] + full_graph[sr_sel[i-1]][sr_sorted_before[0]]; 264 | } 265 | for (int j = 1; j < m; j++) { 266 | cost[0][j] = cost[0][j-1] + full_graph[sr_sel[0]][sr_sorted_before[j-1]]; 267 | } 268 | 269 | // Fill in the rest of the cost matrix 270 | for (int i = 1; i < n; i++) { 271 | for (int j = 1; j < m; j++) { 272 | // float d = abs(sr_sel[i] - sr_sorted_before[j]); 273 | float d = full_graph[sr_sel[i-1]][sr_sorted_before[j-1]]; 274 | cost[i][j] = d + min(cost[i-1][j], min(cost[i][j-1], cost[i-1][j-1])); 275 | } 276 | } 277 | // Return the DTW distance as a similarity measure 278 | float similarity = exp(-cost[n-1][m-1] / (n+m)); 279 | return similarity; 280 | } 281 | 282 | vector sortSubRgi_bfs(vector> full_graph,vector sr_sel,vector sr_sorted_before,int sr_bot) 283 | { 284 | float best_score = -1e5; 285 | vector best_path; 286 | sort(sr_sel.begin(), sr_sel.end()); 287 | do{ 288 | float dis_to_ini = abs(odom.pose.pose.position.x - center_points[sr_sel[0]].x) + abs(odom.pose.pose.position.y - center_points[sr_sel[0]].y); 289 | // float score = 290 | // 7 * similarity_dtw(full_graph, sr_sel,sr_sorted_before) - route_length(full_graph, sr_sel)/(sr_sel.size()+1) 291 | // - 4 * full_graph[sr_bot][sr_sel[0]]; 292 | float score = 293 | 7 * similarity_dtw(full_graph, sr_sel,sr_sorted_before) - route_length(full_graph, sr_sel)/(sr_sel.size()+1) 294 | - 2 * dis_to_ini; 295 | if (score > best_score) { 296 | best_score = score; 297 | best_path = sr_sel; 298 | } 299 | } while (next_permutation(sr_sel.begin(), sr_sel.end())); 300 | return best_path; 301 | } 302 | 303 | vector sortSubRgi_asa(vector> full_graph,vector sr_sel,vector sr_sorted_before,int sr_bot) 304 | { 305 | float init_temp = 1e3; 306 | float final_temp = 0.1; 307 | float temp = init_temp; 308 | float eta = 1; 309 | vector best_path = sr_sel; 310 | float best_score = -1e3; 311 | int count = 0; 312 | int n_ite = 2e4; 313 | random_device rd; 314 | mt19937 gen(rd()); 315 | uniform_real_distribution<> dis(0, 1); 316 | while (temp > 1e5 && count < n_ite) 317 | { 318 | //generate new path 319 | vector new_path = sr_sel; 320 | int index1 = round(dis(gen) * (new_path.size()-1)); 321 | int index2 = round(dis(gen) * (new_path.size()-1)); 322 | swap(new_path[index1], new_path[index2]); 323 | 324 | //calculate score difference 325 | float score = 326 | 6 * similarity_dtw(full_graph, new_path,sr_sorted_before) 327 | - route_length(full_graph, new_path)/(new_path.size()+1) 328 | - 4 * full_graph[sr_bot][new_path[0]]; 329 | float delta = score - best_score; 330 | // update best score and path 331 | if (delta > 0) { 332 | best_score = score; 333 | best_path = new_path; 334 | } else { 335 | float prob = exp(delta / temp); 336 | if (prob > (float)dis(gen)) { 337 | best_score = score; 338 | best_path = new_path; 339 | } 340 | } 341 | eta = min( exp(0.002 * ( float(count)/n_ite -1 )), 0.999 ); 342 | temp *= eta; 343 | count++; 344 | } 345 | return best_path; 346 | } 347 | 348 | // Classify Frontier Points ============================================== 349 | vector> classifyFpoints(vector sr_sorted) 350 | { 351 | vector> fpoint_sorted; 352 | double grid_width = map_width / 3; 353 | double grid_height = map_height / 3; 354 | 355 | for(int i=0; i fpoints_in_grid; 358 | int grid_num = sr_sorted[i]; 359 | double x = map_origin.x + (grid_num % 3) * grid_width + grid_width / 2; 360 | double y = map_origin.y + (grid_num / 3) * grid_height + grid_height / 2; 361 | geometry_msgs::Point center; 362 | center.x = x; 363 | center.y = y; 364 | center.z = 0.0; 365 | for(int j=0; j("/map_topic", map_topic, "/map"); 387 | ros::param::param("/odom_topic", odom_topic, "/odom"); 388 | ros::Subscriber mapsub = nh.subscribe(map_topic, 10, mapCallBack); 389 | ros::Subscriber odomsub = nh.subscribe(odom_topic, 10, odomCallBack); 390 | ros::Subscriber fpoint_sub = nh.subscribe("/filtered_points", 10, fpCallBack); 391 | ros::Publisher subregion_pub = nh.advertise("/subregion_marker", 10); 392 | ros::Publisher grid_pub = nh.advertise("/grid_marker", 10); 393 | ros::Publisher cand_fp_pub = nh.advertise("/candidate_fpoint", 10); 394 | 395 | // ros::Timer updatesr_timer_ = nh.createTimer(ros::Duration(1), updatesrCallback); 396 | 397 | ros::Rate rate(10); 398 | 399 | // wait until map is received 400 | while (mapData.header.seq<1 or mapData.data.size()<100) {ros::spinOnce(); rate.sleep();} 401 | 402 | // initialize the subregion distance matrix 403 | vector> full_graph; 404 | for(int i=0;i<9;i++){ 405 | vector one_line; 406 | for(int j=0;j<9;j++){ 407 | float dx = pow((i % 3 - j % 3),2); 408 | float dy = pow((i / 3 - j / 3),2); 409 | one_line.push_back(dx + dy); 410 | } 411 | full_graph.push_back(one_line); 412 | } 413 | 414 | vector sr_sorted_before(3); 415 | 416 | // Main loop 417 | while (ros::ok()) 418 | { 419 | ros::Time start = ros::Time::now(); 420 | 421 | vector sr_sel = selectSubRegi(); 422 | 423 | vector sr_sorted; 424 | if(sr_sel.size()<7) 425 | sr_sorted = sortSubRgi_bfs(full_graph,sr_sel,sr_sorted_before,sr_bot); 426 | else 427 | sr_sorted = sortSubRgi_asa(full_graph,sr_sel,sr_sorted_before,sr_bot); 428 | sr_sorted_before = sr_sorted; 429 | 430 | vector> fpoint_sorted = classifyFpoints(sr_sorted); 431 | 432 | ros::Duration duration = ros::Time::now() - start; 433 | ROS_INFO_THROTTLE(3, "Time for subregion arrangement: %f", duration.toSec()); 434 | 435 | // publish frontier point in the first subregion 436 | for(int i=0; i