├── CMakeLists.txt ├── README.md ├── config ├── global_costmap_params_tb3_0.yaml ├── global_costmap_params_tb3_1.yaml ├── local_costmap_params_tb3_0.yaml ├── local_costmap_params_tb3_1.yaml ├── mapper_params_online_sync.yaml ├── single_tb_FE.rviz └── two_tb.rviz ├── launch ├── modified_move_base.launch ├── multi_robot_map_merge.launch ├── single_tb_FE.launch ├── slam_online_synch.launch ├── spawn_robots.launch ├── tb3_0.launch ├── tb3_1.launch └── two_tb_exploration.launch ├── package.xml ├── src ├── map_expansion.cpp ├── single_tb_FE.cpp ├── tb3_0_FE.cpp └── tb3_1_FE.cpp ├── srv ├── tb3_0_start.srv └── tb3_1_start.srv ├── urdf ├── tb3_0.gazebo.xacro ├── tb3_0.urdf.xacro ├── tb3_1.gazebo.xacro └── tb3_1.urdf.xacro └── worlds └── bookstore.world /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.9) 2 | project(multi_robot_exploration) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | actionlib 12 | message_generation 13 | message_runtime 14 | move_base_msgs 15 | roscpp 16 | std_msgs 17 | tf2 18 | tf2_ros 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | # find_package(Boost REQUIRED COMPONENTS system) 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a exec_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | # add_message_files( 55 | # FILES 56 | # Message1.msg 57 | # Message2.msg 58 | # ) 59 | 60 | ## Generate services in the 'srv' folder 61 | add_service_files( 62 | FILES 63 | tb3_0_start.srv 64 | tb3_1_start.srv 65 | ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | generate_messages( 76 | DEPENDENCIES 77 | std_msgs # Or other packages containing msgs 78 | ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if your package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | # INCLUDE_DIRS include 111 | # LIBRARIES multi_turtlebot_slam 112 | CATKIN_DEPENDS 113 | actionlib 114 | message_generation 115 | message_runtime 116 | move_base_msgs 117 | roscpp 118 | std_msgs 119 | tf2 120 | tf2_ros 121 | # DEPENDS system_lib 122 | ) 123 | 124 | ########### 125 | ## Build ## 126 | ########### 127 | 128 | ## Specify additional locations of header files 129 | ## Your package locations should be listed before other locations 130 | include_directories( 131 | # include 132 | ${catkin_INCLUDE_DIRS} 133 | ) 134 | 135 | ## Declare a C++ library 136 | # add_library(${PROJECT_NAME} 137 | # src/${PROJECT_NAME}/multi_turtlebot_slam.cpp 138 | # ) 139 | 140 | ## Add cmake target dependencies of the library 141 | ## as an example, code may need to be generated before libraries 142 | ## either from message generation or dynamic reconfigure 143 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 144 | 145 | ## Declare a C++ executable 146 | ## With catkin_make all packages are built within a single CMake context 147 | ## The recommended prefix ensures that target names across packages don't collide 148 | add_executable(map_node src/map_expansion.cpp) 149 | add_executable(single_tb_FE src/single_tb_FE.cpp) 150 | add_executable(tb3_0_FE src/tb3_0_FE.cpp) 151 | add_executable(tb3_1_FE src/tb3_1_FE.cpp) 152 | 153 | ## Rename C++ executable without prefix 154 | ## The above recommended prefix causes long target names, the following renames the 155 | ## target back to the shorter version for ease of user use 156 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 157 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 158 | 159 | ## Add cmake target dependencies of the executable 160 | ## same as for the library above 161 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 162 | 163 | ## Specify libraries to link a library or executable target against 164 | target_link_libraries( map_node 165 | ${catkin_LIBRARIES} 166 | ) 167 | target_link_libraries( single_tb_FE 168 | ${catkin_LIBRARIES} 169 | ) 170 | target_link_libraries( tb3_0_FE 171 | ${catkin_LIBRARIES} 172 | ) 173 | target_link_libraries( tb3_1_FE 174 | ${catkin_LIBRARIES} 175 | ) 176 | 177 | ############# 178 | ## Install ## 179 | ############# 180 | 181 | # all install targets should use catkin DESTINATION variables 182 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 183 | 184 | ## Mark executable scripts (Python etc.) for installation 185 | ## in contrast to setup.py, you can choose the destination 186 | # catkin_install_python(PROGRAMS 187 | # scripts/my_python_script 188 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 189 | # ) 190 | 191 | ## Mark executables for installation 192 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 193 | install(TARGETS 194 | map_node 195 | single_tb_FE 196 | tb3_0_FE 197 | tb3_1_FE 198 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 199 | ) 200 | # install(TARGETS ${PROJECT_NAME}_map_node 201 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 202 | # ) 203 | # install(TARGETS ${PROJECT_NAME}_tb3_0_FE 204 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 205 | # ) 206 | # install(TARGETS ${PROJECT_NAME}_tb3_1_FE 207 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 208 | # ) 209 | 210 | 211 | ## Mark libraries for installation 212 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 213 | # install(TARGETS ${PROJECT_NAME} 214 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 215 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 216 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 217 | # ) 218 | 219 | ## Mark cpp header files for installation 220 | # install(DIRECTORY include/${PROJECT_NAME}/ 221 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 222 | # FILES_MATCHING PATTERN "*.h" 223 | # PATTERN ".svn" EXCLUDE 224 | # ) 225 | 226 | ## Mark other files for installation (e.g. launch and bag files, etc.) 227 | # install(FILES 228 | # # myfile1 229 | # # myfile2 230 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 231 | # ) 232 | 233 | ############# 234 | ## Testing ## 235 | ############# 236 | 237 | ## Add gtest based cpp test target and link libraries 238 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_multi_turtlebot_slam.cpp) 239 | # if(TARGET ${PROJECT_NAME}-test) 240 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 241 | # endif() 242 | 243 | ## Add folders to be run by python nosetests 244 | # catkin_add_nosetests(test) 245 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Kailey Smith 2 | # Multi-Robot Exploration and Map Merging 3 | 4 | ![real_FE_noisy](https://user-images.githubusercontent.com/70979347/111921896-0b3fe980-8a65-11eb-9349-c4512cfde041.gif) 5 | 6 | ## Project Description 7 | The goal is to use multiple robots to autonomously explore an environment and create one global merged map comprised of each's robot individual map. This can be achived with or without knwoledge of the robot's initial positions. 8 | 9 | Frontier Exploration was implemented as the autonomous navigation algorithm. Map merging was achieved by modifying the [multirobot_map_merge](http://wiki.ros.org/multirobot_map_merge) node. 10 | 11 | Currently this repository contains the software necessary to run frontier exploration and map merging of multiple robots in simulation using Gazebo. It also contains the software necessary to run frontier exploration on one actual Turtlebot3. Actively developing multi-robot exploration and map merging on actual Turtlebot3s. Also developing modularity for functionality with any number of robots. 12 | 13 | Note: The original multirobot_map_merge node was written to run gmapping, but for this package the gmapping functionality has been removed in favor of using `slam_toolbox`. To achieve map merging however, each robot's occupancy grid must be the same size and have the same origin. To compensate for this, the `map_expansion` node was written which creates a new, larger map and inserts `slam_toolbox`'s data into the new map. 14 | 15 | To see this package in action, please view my portfolio and post and video deomonstration here: https://gingineer95.github.io/2021/03/21/multirobot-map-merge/ 16 | 17 | ## 3rd Party Packages 18 | - [Multirobot_map_merge](http://wiki.ros.org/multirobot_map_merge): Node that merges the individual maps into one gloval map 19 | - [Aws-robomaker-bookstore-world](https://github.com/aws-robotics/aws-robomaker-bookstore-world): Gazebo world of a bookstore 20 | - [Turtlebot3](https://emanual.robotis.com/docs/en/platform/turtlebot3/quick-start/) (navigation, gazebo, teleop and bringup) 21 | - [Slam_toolbox](https://github.com/SteveMacenski/slam_toolbox): Used for simultaneous localization and mapping 22 | - [Teleop_twist_keyboard](http://wiki.ros.org/teleop_twist_keyboard): Generic keyboard teleop for twist robots 23 | - [Gazebo_ros](http://wiki.ros.org/gazebo_ros) 24 | 25 | ## Installation Instructions 26 | - This package is called multi_robot_exploration 27 | - I dont have a rosinstall at the moment (my apologies, coming very soon) but for now please use the packages above for referance 28 | 29 | ## Getting Started 30 | ### Multi-Robot Exploration and Map Merging 31 | 32 | ![2tb_FE](https://user-images.githubusercontent.com/70979347/111922117-22cba200-8a66-11eb-8fbc-e9a0257c7e63.gif) 33 | 34 | 35 | #### Known Initial Robot Positions 36 | - This package is initially set up to run in simulation given that you know where the robot's spawn position. 37 | - To run multi-robot exploration and map merging in a simulated bookstore, `source` your workspace and run `roslaunch multi_robot_exploration two_tb_exploration.launch` 38 | - You should see two Turtlebots spawn in Gazebo and Rviz. In Rviz you should see three maps being published: `tb3_0/map`, `tb3_1/map` and `map` 39 | - Please give the simulation a minuite to load 40 | - You'll know everything is up and running when the merged map appears (published on the `map` topic) 41 | - `tb3_0/map` and `tb3_1/map` are the individual robot maps which are published from `slam_toolbox` 42 | - The robots require a start service be called before they start executing frontier exploration 43 | - To start frontier exploration for both robots run `rosservice call /tb3_0_start` 44 | and `rosservice call /tb3_1_start` 45 | - You'll know this has been executed sucessfully when red and purple goal arrows appear in Rviz, as well as the robot's planned path 46 | - Also when the robots start moving (of course) 47 | - Now sit back and watch the robots explore the bookstore! 48 | 49 | #### Unknown Initial Robot Positions 50 | 51 | ![rviz_unknown_pos](https://user-images.githubusercontent.com/70979347/111922165-57d7f480-8a66-11eb-8364-e0724359a6b5.png) 52 | 53 | 54 | - For unknown initial positions, the robots must spawn relatively close to eachother. This is because the multirobot_map_merging node needs a sufficinet amount of maps to overlap in order to use a feature detection algorithm to stitch the individial maps together. 55 | - See the [multirobot_map_merge](http://wiki.ros.org/multirobot_map_merge) for more documentation. 56 | - To run `source` your workspace and run `roslaunch multi_robot_exploration two_tb_exploration.launch known_initial_pos:=false first_tb3_x_pos:=-1.0 first_tb3_y_pos:=-6.0 second_tb3_x_pos:=-1.5 second_tb3_y_pos:=-6.0` 57 | - You can edit the initial position values directly in the launch file instead of you would like 58 | - As with the known initial positions, you should see two Turtlebots spawn in Gazebo and Rviz 59 | - Again, the robots require a start service be called before they start executing frontier exploration 60 | - To start frontier exploration for both robots run `rosservice call /tb3_0_start` 61 | and `rosservice call /tb3_1_start` 62 | 63 | ### Frontier Exploration on an actual Turtlebot3 64 | - Please see the [Turtlebot3](https://emanual.robotis.com/docs/en/platform/turtlebot3/quick-start/) on how to set up your Turtlebot if you haven't already 65 | - Note: This package was written to work with the `burger` Turtlebot model. If you would like to use another model, you will have to add / edit your own urdfs and any referance to the `burger` Turtlebot in the current launch / config files. 66 | - Start a `roscore` in your PC 67 | - SSH into your Turtlebot and run `roslaunch turtlebot3_bringup turtlebot3_robot.launch` 68 | - To start frontier exploration run `roslaunch single_tb_FE.launch` on a terminal on your PC. 69 | - Enjoy the soothing sights and sounds of your Turtlebot exploring real live frontiers 70 | 71 | **Note: For best results, use your Turtlebot on smooth surfaces as opposed to carpet** 72 | 73 | ## Future Development 74 | - As mentioned above, I am currently working on making this software more modular so that adding more robots to this operation is as simple as adding another namespace 75 | - I am also working on running frontier exploration and map mergering with the software I have for two robots right now 76 | - Build on the existing multirobot_map_merge node so that robots can always start from unknown initial positions. The idea is to have the robots start exploring without any knowledge of each other while in the background I run a feature matching algorthim. Once enough of two robot's map features match, I will create a merged map and then have robots run frontier exploration on the merged map. This way, both robots can continue searching frontiers without re-exploring areas that have already been covered. -------------------------------------------------------------------------------- /config/global_costmap_params_tb3_0.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: tb3_0/map 3 | robot_base_frame: tb3_0/base_footprint 4 | 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | transform_tolerance: 0.5 8 | 9 | # static_map: false 10 | # rolling_window: true 11 | static_map: true 12 | 13 | -------------------------------------------------------------------------------- /config/global_costmap_params_tb3_1.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: tb3_1/map 3 | robot_base_frame: tb3_1/base_footprint 4 | 5 | update_frequency: 5.0 #was 10.0 6 | publish_frequency: 5.0 #was 10.0 7 | transform_tolerance: 0.5 8 | 9 | # static_map: false 10 | # rolling_window: true 11 | static_map: true 12 | 13 | # inflater_layer: 14 | # inflation_radius: 0.55 #default of 0.55 15 | # cost_scaling_factor: 2.5 16 | 17 | -------------------------------------------------------------------------------- /config/local_costmap_params_tb3_0.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: tb3_0/odom 3 | robot_base_frame: tb3_0/base_footprint 4 | 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | transform_tolerance: 0.5 8 | 9 | static_map: false 10 | rolling_window: true 11 | width: 3 12 | height: 3 13 | resolution: 0.05 14 | 15 | -------------------------------------------------------------------------------- /config/local_costmap_params_tb3_1.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: tb3_1/odom 3 | robot_base_frame: tb3_1/base_footprint 4 | 5 | update_frequency: 10.0 6 | publish_frequency: 10.0 7 | transform_tolerance: 0.5 8 | 9 | static_map: false 10 | rolling_window: true 11 | width: 3 12 | height: 3 13 | resolution: 0.05 14 | 15 | -------------------------------------------------------------------------------- /config/mapper_params_online_sync.yaml: -------------------------------------------------------------------------------- 1 | # Plugin params 2 | solver_plugin: solver_plugins::CeresSolver 3 | ceres_linear_solver: SPARSE_NORMAL_CHOLESKY 4 | ceres_preconditioner: SCHUR_JACOBI 5 | ceres_trust_strategy: LEVENBERG_MARQUARDT 6 | ceres_dogleg_type: TRADITIONAL_DOGLEG 7 | ceres_loss_function: None 8 | 9 | # ROS Parameters 10 | odom_frame: odom 11 | map_frame: map 12 | base_frame: base_footprint 13 | scan_topic: /scan 14 | mode: mapping #localization 15 | 16 | # if you'd like to immediately start continuing a map at a given pose 17 | # or at the dock, but they are mutually exclusive, if pose is given 18 | # will use pose 19 | #map_file_name: test_steve 20 | #map_start_pose: [0.0, 0.0, 0.0] 21 | #map_start_at_dock: true 22 | 23 | debug_logging: false 24 | throttle_scans: 1 25 | transform_publish_period: 0.02 #if 0 never publishes odometry 26 | map_update_interval: 5.0 27 | resolution: 0.05 28 | max_laser_range: 20.0 #for rastering images 29 | minimum_time_interval: 0.5 30 | transform_timeout: 0.2 31 | tf_buffer_duration: 30. 32 | stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps 33 | enable_interactive_mode: true 34 | 35 | # General Parameters 36 | use_scan_matching: true 37 | use_scan_barycenter: true 38 | minimum_travel_distance: 0.5 39 | minimum_travel_heading: 0.5 40 | scan_buffer_size: 10 41 | scan_buffer_maximum_scan_distance: 10 42 | link_match_minimum_response_fine: 0.1 43 | link_scan_maximum_distance: 1.5 44 | loop_search_maximum_distance: 3.0 45 | do_loop_closing: true 46 | loop_match_minimum_chain_size: 10 47 | loop_match_maximum_variance_coarse: 3.0 48 | loop_match_minimum_response_coarse: 0.35 49 | loop_match_minimum_response_fine: 0.45 50 | 51 | # Correlation Parameters - Correlation Parameters 52 | correlation_search_space_dimension: 0.5 53 | correlation_search_space_resolution: 0.01 54 | correlation_search_space_smear_deviation: 0.1 55 | 56 | # Correlation Parameters - Loop Closure Parameters 57 | loop_search_space_dimension: 8.0 58 | loop_search_space_resolution: 0.05 59 | loop_search_space_smear_deviation: 0.03 60 | 61 | # Scan Matcher Parameters 62 | distance_variance_penalty: 0.5 63 | angle_variance_penalty: 1.0 64 | 65 | fine_search_angle_offset: 0.00349 66 | coarse_search_angle_offset: 0.349 67 | coarse_angle_resolution: 0.0349 68 | minimum_angle_penalty: 0.9 69 | minimum_distance_penalty: 0.5 70 | use_response_expansion: true 71 | -------------------------------------------------------------------------------- /config/single_tb_FE.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /TF1/Tree1 9 | - /Map1/Status1 10 | - /Global Map1/Costmap1 11 | - /Global Map1/Planner1 12 | - /Local Map1/Polygon1 13 | - /Local Map1/Costmap1 14 | - /Local Map1/Planner1 15 | - /FE_edges_Map1/Status1 16 | Splitter Ratio: 0.5 17 | Tree Height: 820 18 | - Class: rviz/Selection 19 | Name: Selection 20 | - Class: rviz/Tool Properties 21 | Expanded: 22 | - /2D Pose Estimate1 23 | - /2D Nav Goal1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.5886790156364441 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: LaserScan 36 | Preferences: 37 | PromptSaveOnExit: true 38 | Toolbars: 39 | toolButtonStyle: 2 40 | Visualization Manager: 41 | Class: "" 42 | Displays: 43 | - Alpha: 0.5 44 | Cell Size: 1 45 | Class: rviz/Grid 46 | Color: 160; 160; 164 47 | Enabled: true 48 | Line Style: 49 | Line Width: 0.029999999329447746 50 | Value: Lines 51 | Name: Grid 52 | Normal Cell Count: 0 53 | Offset: 54 | X: 0 55 | Y: 0 56 | Z: 0 57 | Plane: XY 58 | Plane Cell Count: 20 59 | Reference Frame: 60 | Value: true 61 | - Alpha: 1 62 | Class: rviz/RobotModel 63 | Collision Enabled: false 64 | Enabled: true 65 | Links: 66 | All Links Enabled: true 67 | Expand Joint Details: false 68 | Expand Link Details: false 69 | Expand Tree: false 70 | Link Tree Style: Links in Alphabetic Order 71 | base_footprint: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | base_link: 76 | Alpha: 1 77 | Show Axes: false 78 | Show Trail: false 79 | Value: true 80 | base_scan: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | caster_back_link: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | imu_link: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | wheel_left_link: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | wheel_right_link: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | Name: RobotModel 105 | Robot Description: robot_description 106 | TF Prefix: "" 107 | Update Interval: 0 108 | Value: true 109 | Visual Enabled: true 110 | - Class: rviz/TF 111 | Enabled: false 112 | Frame Timeout: 15 113 | Frames: 114 | All Enabled: false 115 | Marker Alpha: 1 116 | Marker Scale: 1 117 | Name: TF 118 | Show Arrows: true 119 | Show Axes: true 120 | Show Names: false 121 | Tree: 122 | {} 123 | Update Interval: 0 124 | Value: false 125 | - Alpha: 1 126 | Autocompute Intensity Bounds: true 127 | Autocompute Value Bounds: 128 | Max Value: 10 129 | Min Value: -10 130 | Value: true 131 | Axis: Z 132 | Channel Name: intensity 133 | Class: rviz/LaserScan 134 | Color: 0; 255; 0 135 | Color Transformer: FlatColor 136 | Decay Time: 0 137 | Enabled: true 138 | Invert Rainbow: false 139 | Max Color: 255; 255; 255 140 | Max Intensity: 13069 141 | Min Color: 0; 0; 0 142 | Min Intensity: 28 143 | Name: LaserScan 144 | Position Transformer: XYZ 145 | Queue Size: 10 146 | Selectable: true 147 | Size (Pixels): 3 148 | Size (m): 0.030000001192092896 149 | Style: Flat Squares 150 | Topic: /scan 151 | Unreliable: false 152 | Use Fixed Frame: true 153 | Use rainbow: true 154 | Value: true 155 | - Class: rviz/Image 156 | Enabled: false 157 | Image Topic: /raspicam_node/image 158 | Max Value: 1 159 | Median window: 5 160 | Min Value: 0 161 | Name: Image 162 | Normalize Range: true 163 | Queue Size: 2 164 | Transport Hint: compressed 165 | Unreliable: false 166 | Value: false 167 | - Alpha: 0.699999988079071 168 | Class: rviz/Map 169 | Color Scheme: map 170 | Draw Behind: false 171 | Enabled: false 172 | Name: Map 173 | Topic: /map 174 | Unreliable: false 175 | Use Timestamp: false 176 | Value: false 177 | - Alpha: 1 178 | Buffer Length: 1 179 | Class: rviz/Path 180 | Color: 0; 0; 0 181 | Enabled: true 182 | Head Diameter: 0.30000001192092896 183 | Head Length: 0.20000000298023224 184 | Length: 0.30000001192092896 185 | Line Style: Lines 186 | Line Width: 0.029999999329447746 187 | Name: Planner Plan 188 | Offset: 189 | X: 0 190 | Y: 0 191 | Z: 0 192 | Pose Color: 255; 85; 255 193 | Pose Style: None 194 | Queue Size: 10 195 | Radius: 0.029999999329447746 196 | Shaft Diameter: 0.10000000149011612 197 | Shaft Length: 0.10000000149011612 198 | Topic: /move_base/NavfnROS/plan 199 | Unreliable: false 200 | Value: true 201 | - Class: rviz/Group 202 | Displays: 203 | - Alpha: 0.699999988079071 204 | Class: rviz/Map 205 | Color Scheme: costmap 206 | Draw Behind: true 207 | Enabled: true 208 | Name: Costmap 209 | Topic: /move_base/global_costmap/costmap 210 | Unreliable: false 211 | Use Timestamp: false 212 | Value: true 213 | - Alpha: 1 214 | Buffer Length: 1 215 | Class: rviz/Path 216 | Color: 255; 0; 0 217 | Enabled: true 218 | Head Diameter: 0.30000001192092896 219 | Head Length: 0.20000000298023224 220 | Length: 0.30000001192092896 221 | Line Style: Lines 222 | Line Width: 0.029999999329447746 223 | Name: Planner 224 | Offset: 225 | X: 0 226 | Y: 0 227 | Z: 0 228 | Pose Color: 255; 85; 255 229 | Pose Style: None 230 | Queue Size: 10 231 | Radius: 0.029999999329447746 232 | Shaft Diameter: 0.10000000149011612 233 | Shaft Length: 0.10000000149011612 234 | Topic: /move_base/DWAPlannerROS/global_plan 235 | Unreliable: false 236 | Value: true 237 | Enabled: false 238 | Name: Global Map 239 | - Class: rviz/Group 240 | Displays: 241 | - Alpha: 1 242 | Class: rviz/Polygon 243 | Color: 0; 0; 0 244 | Enabled: true 245 | Name: Polygon 246 | Queue Size: 10 247 | Topic: /move_base/local_costmap/footprint 248 | Unreliable: false 249 | Value: true 250 | - Alpha: 0.699999988079071 251 | Class: rviz/Map 252 | Color Scheme: costmap 253 | Draw Behind: false 254 | Enabled: true 255 | Name: Costmap 256 | Topic: /move_base/local_costmap/costmap 257 | Unreliable: false 258 | Use Timestamp: false 259 | Value: true 260 | - Alpha: 1 261 | Buffer Length: 1 262 | Class: rviz/Path 263 | Color: 255; 255; 0 264 | Enabled: true 265 | Head Diameter: 0.30000001192092896 266 | Head Length: 0.20000000298023224 267 | Length: 0.30000001192092896 268 | Line Style: Lines 269 | Line Width: 0.029999999329447746 270 | Name: Planner 271 | Offset: 272 | X: 0 273 | Y: 0 274 | Z: 0 275 | Pose Color: 255; 85; 255 276 | Pose Style: None 277 | Queue Size: 10 278 | Radius: 0.029999999329447746 279 | Shaft Diameter: 0.10000000149011612 280 | Shaft Length: 0.10000000149011612 281 | Topic: /move_base/DWAPlannerROS/local_plan 282 | Unreliable: false 283 | Value: true 284 | Enabled: false 285 | Name: Local Map 286 | - Alpha: 1 287 | Arrow Length: 0.05000000074505806 288 | Axes Length: 0.30000001192092896 289 | Axes Radius: 0.009999999776482582 290 | Class: rviz/PoseArray 291 | Color: 0; 192; 0 292 | Enabled: true 293 | Head Length: 0.07000000029802322 294 | Head Radius: 0.029999999329447746 295 | Name: Amcl Particles 296 | Queue Size: 10 297 | Shaft Length: 0.23000000417232513 298 | Shaft Radius: 0.009999999776482582 299 | Shape: Arrow (Flat) 300 | Topic: /particlecloud 301 | Unreliable: false 302 | Value: true 303 | - Alpha: 1 304 | Axes Length: 1 305 | Axes Radius: 0.10000000149011612 306 | Class: rviz/Pose 307 | Color: 170; 170; 255 308 | Enabled: true 309 | Head Length: 0.30000001192092896 310 | Head Radius: 0.25 311 | Name: Goal 312 | Queue Size: 10 313 | Shaft Length: 0.5 314 | Shaft Radius: 0.10000000149011612 315 | Shape: Arrow 316 | Topic: /move_base/current_goal 317 | Unreliable: false 318 | Value: true 319 | - Alpha: 0.699999988079071 320 | Class: rviz/Map 321 | Color Scheme: map 322 | Draw Behind: false 323 | Enabled: true 324 | Name: FE_edges_Map 325 | Topic: /edges_map 326 | Unreliable: false 327 | Use Timestamp: false 328 | Value: true 329 | Enabled: true 330 | Global Options: 331 | Background Color: 48; 48; 48 332 | Default Light: true 333 | Fixed Frame: map 334 | Frame Rate: 30 335 | Name: root 336 | Tools: 337 | - Class: rviz/MoveCamera 338 | - Class: rviz/Interact 339 | Hide Inactive Objects: true 340 | - Class: rviz/Select 341 | - Class: rviz/SetInitialPose 342 | Theta std deviation: 0.2617993950843811 343 | Topic: /initialpose 344 | X std deviation: 0.5 345 | Y std deviation: 0.5 346 | - Class: rviz/SetGoal 347 | Topic: /move_base_simple/goal 348 | - Class: rviz/Measure 349 | - Class: rviz/FocusCamera 350 | Value: true 351 | Views: 352 | Current: 353 | Angle: -1.5807958841323853 354 | Class: rviz/TopDownOrtho 355 | Enable Stereo Rendering: 356 | Stereo Eye Separation: 0.05999999865889549 357 | Stereo Focal Distance: 1 358 | Swap Stereo Eyes: false 359 | Value: false 360 | Invert Z Axis: false 361 | Name: Current View 362 | Near Clip Distance: 0.009999999776482582 363 | Scale: 68.93179321289062 364 | Target Frame: 365 | X: -2.5414304733276367 366 | Y: 0.8473602533340454 367 | Saved: ~ 368 | Window Geometry: 369 | Displays: 370 | collapsed: false 371 | Height: 1043 372 | Hide Left Dock: false 373 | Hide Right Dock: true 374 | Image: 375 | collapsed: false 376 | QMainWindow State: 000000ff00000000fd000000040000000000000221000003bdfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003bd000000c700fffffffb0000000a0049006d0061006700650000000317000000cc0000001600fffffffb0000000a0049006d0061006700650000000330000000ce0000000000000000000000010000010f000003a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003a0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000559000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 377 | Selection: 378 | collapsed: false 379 | Time: 380 | collapsed: false 381 | Tool Properties: 382 | collapsed: false 383 | Views: 384 | collapsed: true 385 | Width: 1920 386 | X: 0 387 | Y: 0 388 | -------------------------------------------------------------------------------- /config/two_tb.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /tb3_0 Local Map1/Costmap1 8 | - /tb3_0 Local Map1/Polygon1 9 | - /tb3_0 Gocal Map1/Costmap1 10 | - /tb3_0 Gocal Map1/Polygon1 11 | - /tb3_1 Local Map1/Costmap1 12 | - /tb3_1 Local Map1/Polygon1 13 | - /tb3_1 Global Map1/Costmap1 14 | - /tb3_1 Global Map1/Polygon1 15 | Splitter Ratio: 0.36039602756500244 16 | Tree Height: 752 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.5886790156364441 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: LaserScan 36 | Preferences: 37 | PromptSaveOnExit: true 38 | Toolbars: 39 | toolButtonStyle: 2 40 | Visualization Manager: 41 | Class: "" 42 | Displays: 43 | - Alpha: 0.5 44 | Cell Size: 0.44999998807907104 45 | Class: rviz/Grid 46 | Color: 160; 160; 164 47 | Enabled: true 48 | Line Style: 49 | Line Width: 0.029999999329447746 50 | Value: Lines 51 | Name: Grid 52 | Normal Cell Count: 0 53 | Offset: 54 | X: 0 55 | Y: 0 56 | Z: 0 57 | Plane: XY 58 | Plane Cell Count: 100 59 | Reference Frame: tb3_0/map 60 | Value: true 61 | - Class: rviz/TF 62 | Enabled: false 63 | Frame Timeout: 15 64 | Frames: 65 | All Enabled: false 66 | Marker Alpha: 1 67 | Marker Scale: 1 68 | Name: TF 69 | Show Arrows: true 70 | Show Axes: true 71 | Show Names: true 72 | Tree: 73 | {} 74 | Update Interval: 0 75 | Value: false 76 | - Alpha: 1 77 | Autocompute Intensity Bounds: true 78 | Autocompute Value Bounds: 79 | Max Value: 10 80 | Min Value: -10 81 | Value: true 82 | Axis: Z 83 | Channel Name: intensity 84 | Class: rviz/LaserScan 85 | Color: 255; 0; 0 86 | Color Transformer: FlatColor 87 | Decay Time: 0 88 | Enabled: true 89 | Invert Rainbow: false 90 | Max Color: 255; 255; 255 91 | Max Intensity: 11799 92 | Min Color: 0; 0; 0 93 | Min Intensity: 0 94 | Name: LaserScan 95 | Position Transformer: XYZ 96 | Queue Size: 10 97 | Selectable: true 98 | Size (Pixels): 3 99 | Size (m): 0.019999999552965164 100 | Style: Flat Squares 101 | Topic: /tb3_0/scan 102 | Unreliable: false 103 | Use Fixed Frame: true 104 | Use rainbow: true 105 | Value: true 106 | - Alpha: 1 107 | Autocompute Intensity Bounds: true 108 | Autocompute Value Bounds: 109 | Max Value: 10 110 | Min Value: -10 111 | Value: true 112 | Axis: Z 113 | Channel Name: intensity 114 | Class: rviz/LaserScan 115 | Color: 255; 255; 255 116 | Color Transformer: Intensity 117 | Decay Time: 0 118 | Enabled: true 119 | Invert Rainbow: false 120 | Max Color: 255; 255; 255 121 | Max Intensity: 0 122 | Min Color: 0; 0; 0 123 | Min Intensity: 0 124 | Name: LaserScan 125 | Position Transformer: XYZ 126 | Queue Size: 10 127 | Selectable: true 128 | Size (Pixels): 3 129 | Size (m): 0.009999999776482582 130 | Style: Flat Squares 131 | Topic: /tb3_1/scan 132 | Unreliable: false 133 | Use Fixed Frame: true 134 | Use rainbow: true 135 | Value: true 136 | - Alpha: 1 137 | Class: rviz/RobotModel 138 | Collision Enabled: false 139 | Enabled: true 140 | Links: 141 | All Links Enabled: true 142 | Expand Joint Details: false 143 | Expand Link Details: false 144 | Expand Tree: false 145 | Link Tree Style: Links in Alphabetic Order 146 | tb3_0/base_footprint: 147 | Alpha: 1 148 | Show Axes: false 149 | Show Trail: false 150 | tb3_0/base_link: 151 | Alpha: 1 152 | Show Axes: false 153 | Show Trail: false 154 | Value: true 155 | tb3_0/base_scan: 156 | Alpha: 1 157 | Show Axes: false 158 | Show Trail: false 159 | Value: true 160 | tb3_0/caster_back_link: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | Value: true 165 | tb3_0/imu_link: 166 | Alpha: 1 167 | Show Axes: false 168 | Show Trail: false 169 | tb3_0/wheel_left_link: 170 | Alpha: 1 171 | Show Axes: false 172 | Show Trail: false 173 | Value: true 174 | tb3_0/wheel_right_link: 175 | Alpha: 1 176 | Show Axes: false 177 | Show Trail: false 178 | Value: true 179 | Name: RobotModel 180 | Robot Description: tb3_0/robot_description 181 | TF Prefix: tb3_0 182 | Update Interval: 0 183 | Value: true 184 | Visual Enabled: true 185 | - Alpha: 1 186 | Class: rviz/RobotModel 187 | Collision Enabled: false 188 | Enabled: true 189 | Links: 190 | All Links Enabled: true 191 | Expand Joint Details: false 192 | Expand Link Details: false 193 | Expand Tree: false 194 | Link Tree Style: Links in Alphabetic Order 195 | tb3_1/base_footprint: 196 | Alpha: 1 197 | Show Axes: false 198 | Show Trail: false 199 | tb3_1/base_link: 200 | Alpha: 1 201 | Show Axes: false 202 | Show Trail: false 203 | Value: true 204 | tb3_1/base_scan: 205 | Alpha: 1 206 | Show Axes: false 207 | Show Trail: false 208 | Value: true 209 | tb3_1/caster_back_link: 210 | Alpha: 1 211 | Show Axes: false 212 | Show Trail: false 213 | Value: true 214 | tb3_1/imu_link: 215 | Alpha: 1 216 | Show Axes: false 217 | Show Trail: false 218 | tb3_1/wheel_left_link: 219 | Alpha: 1 220 | Show Axes: false 221 | Show Trail: false 222 | Value: true 223 | tb3_1/wheel_right_link: 224 | Alpha: 1 225 | Show Axes: false 226 | Show Trail: false 227 | Value: true 228 | Name: RobotModel 229 | Robot Description: tb3_1/robot_description 230 | TF Prefix: tb3_1 231 | Update Interval: 0 232 | Value: true 233 | Visual Enabled: true 234 | - Alpha: 0.30000001192092896 235 | Class: rviz/Map 236 | Color Scheme: map 237 | Draw Behind: false 238 | Enabled: false 239 | Name: Map 240 | Topic: /tb3_0/map 241 | Unreliable: false 242 | Use Timestamp: false 243 | Value: false 244 | - Alpha: 0.30000001192092896 245 | Class: rviz/Map 246 | Color Scheme: map 247 | Draw Behind: false 248 | Enabled: false 249 | Name: Map 250 | Topic: /tb3_1/map 251 | Unreliable: false 252 | Use Timestamp: false 253 | Value: false 254 | - Alpha: 0.699999988079071 255 | Class: rviz/Map 256 | Color Scheme: map 257 | Draw Behind: false 258 | Enabled: true 259 | Name: Map 260 | Topic: /map 261 | Unreliable: false 262 | Use Timestamp: false 263 | Value: true 264 | - Class: rviz/Group 265 | Displays: 266 | - Alpha: 0.699999988079071 267 | Class: rviz/Map 268 | Color Scheme: costmap 269 | Draw Behind: false 270 | Enabled: true 271 | Name: Costmap 272 | Topic: /tb3_0/move_base/local_costmap/costmap 273 | Unreliable: false 274 | Use Timestamp: false 275 | Value: true 276 | - Alpha: 1 277 | Class: rviz/Polygon 278 | Color: 25; 255; 0 279 | Enabled: true 280 | Name: Polygon 281 | Queue Size: 10 282 | Topic: /tb3_0/move_base/local_costmap/footprint 283 | Unreliable: false 284 | Value: true 285 | Enabled: false 286 | Name: tb3_0 Local Map 287 | - Class: rviz/Group 288 | Displays: 289 | - Alpha: 0.699999988079071 290 | Class: rviz/Map 291 | Color Scheme: costmap 292 | Draw Behind: false 293 | Enabled: true 294 | Name: Costmap 295 | Topic: /tb3_0/move_base/global_costmap/costmap 296 | Unreliable: false 297 | Use Timestamp: false 298 | Value: true 299 | - Alpha: 1 300 | Class: rviz/Polygon 301 | Color: 25; 255; 0 302 | Enabled: true 303 | Name: Polygon 304 | Queue Size: 10 305 | Topic: /tb3_0/move_base/global_costmap/footprint 306 | Unreliable: false 307 | Value: true 308 | Enabled: false 309 | Name: tb3_0 Gocal Map 310 | - Class: rviz/Group 311 | Displays: 312 | - Alpha: 0.699999988079071 313 | Class: rviz/Map 314 | Color Scheme: costmap 315 | Draw Behind: false 316 | Enabled: true 317 | Name: Costmap 318 | Topic: /tb3_1/move_base/local_costmap/costmap 319 | Unreliable: false 320 | Use Timestamp: false 321 | Value: true 322 | - Alpha: 1 323 | Class: rviz/Polygon 324 | Color: 25; 255; 0 325 | Enabled: true 326 | Name: Polygon 327 | Queue Size: 10 328 | Topic: /tb3_1/move_base/local_costmap/footprint 329 | Unreliable: false 330 | Value: true 331 | Enabled: false 332 | Name: tb3_1 Local Map 333 | - Class: rviz/Group 334 | Displays: 335 | - Alpha: 0.699999988079071 336 | Class: rviz/Map 337 | Color Scheme: costmap 338 | Draw Behind: false 339 | Enabled: true 340 | Name: Costmap 341 | Topic: /tb3_1/move_base/global_costmap/costmap 342 | Unreliable: false 343 | Use Timestamp: false 344 | Value: true 345 | - Alpha: 1 346 | Class: rviz/Polygon 347 | Color: 25; 255; 0 348 | Enabled: true 349 | Name: Polygon 350 | Queue Size: 10 351 | Topic: /tb3_1/move_base/global_costmap/footprint 352 | Unreliable: false 353 | Value: true 354 | Enabled: false 355 | Name: tb3_1 Global Map 356 | - Alpha: 1 357 | Buffer Length: 1 358 | Class: rviz/Path 359 | Color: 25; 255; 0 360 | Enabled: true 361 | Head Diameter: 0.30000001192092896 362 | Head Length: 0.20000000298023224 363 | Length: 0.30000001192092896 364 | Line Style: Lines 365 | Line Width: 0.029999999329447746 366 | Name: tb3_1/Nav_Path 367 | Offset: 368 | X: 0 369 | Y: 0 370 | Z: 0 371 | Pose Color: 255; 85; 255 372 | Pose Style: None 373 | Queue Size: 10 374 | Radius: 0.029999999329447746 375 | Shaft Diameter: 0.10000000149011612 376 | Shaft Length: 0.10000000149011612 377 | Topic: /tb3_1/move_base/NavfnROS/plan 378 | Unreliable: false 379 | Value: true 380 | - Alpha: 1 381 | Axes Length: 1 382 | Axes Radius: 0.10000000149011612 383 | Class: rviz/Pose 384 | Color: 170; 0; 255 385 | Enabled: true 386 | Head Length: 0.30000001192092896 387 | Head Radius: 0.10000000149011612 388 | Name: tb3_1/Goal_Pose 389 | Queue Size: 10 390 | Shaft Length: 1 391 | Shaft Radius: 0.05000000074505806 392 | Shape: Arrow 393 | Topic: /tb3_1/move_base/current_goal 394 | Unreliable: false 395 | Value: true 396 | - Alpha: 1 397 | Buffer Length: 1 398 | Class: rviz/Path 399 | Color: 25; 255; 0 400 | Enabled: false 401 | Head Diameter: 0.30000001192092896 402 | Head Length: 0.20000000298023224 403 | Length: 0.30000001192092896 404 | Line Style: Lines 405 | Line Width: 0.029999999329447746 406 | Name: tb3_1/ROS_Local_Path 407 | Offset: 408 | X: 0 409 | Y: 0 410 | Z: 0 411 | Pose Color: 255; 85; 255 412 | Pose Style: None 413 | Queue Size: 10 414 | Radius: 0.029999999329447746 415 | Shaft Diameter: 0.10000000149011612 416 | Shaft Length: 0.10000000149011612 417 | Topic: /tb3_1/move_base/TrajectoryPlannerROS/local_plan 418 | Unreliable: false 419 | Value: false 420 | - Alpha: 1 421 | Buffer Length: 1 422 | Class: rviz/Path 423 | Color: 25; 255; 0 424 | Enabled: true 425 | Head Diameter: 0.30000001192092896 426 | Head Length: 0.20000000298023224 427 | Length: 0.30000001192092896 428 | Line Style: Lines 429 | Line Width: 0.029999999329447746 430 | Name: tb3_0/Path 431 | Offset: 432 | X: 0 433 | Y: 0 434 | Z: 0 435 | Pose Color: 255; 85; 255 436 | Pose Style: None 437 | Queue Size: 10 438 | Radius: 0.029999999329447746 439 | Shaft Diameter: 0.10000000149011612 440 | Shaft Length: 0.10000000149011612 441 | Topic: /tb3_0/move_base/NavfnROS/plan 442 | Unreliable: false 443 | Value: true 444 | - Alpha: 1 445 | Axes Length: 1 446 | Axes Radius: 0.10000000149011612 447 | Class: rviz/Pose 448 | Color: 170; 0; 0 449 | Enabled: true 450 | Head Length: 0.30000001192092896 451 | Head Radius: 0.10000000149011612 452 | Name: tb3_0/GoalPose 453 | Queue Size: 10 454 | Shaft Length: 1 455 | Shaft Radius: 0.05000000074505806 456 | Shape: Arrow 457 | Topic: /tb3_0/move_base/current_goal 458 | Unreliable: false 459 | Value: true 460 | Enabled: true 461 | Global Options: 462 | Background Color: 48; 48; 48 463 | Default Light: true 464 | Fixed Frame: tb3_0/map 465 | Frame Rate: 30 466 | Name: root 467 | Tools: 468 | - Class: rviz/Interact 469 | Hide Inactive Objects: true 470 | - Class: rviz/MoveCamera 471 | - Class: rviz/Select 472 | - Class: rviz/FocusCamera 473 | - Class: rviz/Measure 474 | - Class: rviz/SetInitialPose 475 | Theta std deviation: 0.2617993950843811 476 | Topic: /initialpose 477 | X std deviation: 0.5 478 | Y std deviation: 0.5 479 | - Class: rviz/SetGoal 480 | Topic: /move_base_simple/goal 481 | - Class: rviz/PublishPoint 482 | Single click: true 483 | Topic: /clicked_point 484 | Value: true 485 | Views: 486 | Current: 487 | Angle: -1.5749995708465576 488 | Class: rviz/TopDownOrtho 489 | Enable Stereo Rendering: 490 | Stereo Eye Separation: 0.05999999865889549 491 | Stereo Focal Distance: 1 492 | Swap Stereo Eyes: false 493 | Value: false 494 | Invert Z Axis: false 495 | Name: Current View 496 | Near Clip Distance: 0.009999999776482582 497 | Scale: 52.795196533203125 498 | Target Frame: map 499 | X: 0.1654088944196701 500 | Y: -0.9386844635009766 501 | Saved: ~ 502 | Window Geometry: 503 | Displays: 504 | collapsed: false 505 | Height: 1043 506 | Hide Left Dock: false 507 | Hide Right Dock: true 508 | QMainWindow State: 000000ff00000000fd0000000400000000000001bc00000379fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000216000001900000000000000000000000010000010f00000373fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003500000373000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000005be0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 509 | Selection: 510 | collapsed: false 511 | Time: 512 | collapsed: false 513 | Tool Properties: 514 | collapsed: false 515 | Views: 516 | collapsed: true 517 | Width: 1920 518 | X: 0 519 | Y: 0 520 | -------------------------------------------------------------------------------- /launch/modified_move_base.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 | -------------------------------------------------------------------------------- /launch/multi_robot_map_merge.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 | -------------------------------------------------------------------------------- /launch/single_tb_FE.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 | -------------------------------------------------------------------------------- /launch/slam_online_synch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /launch/spawn_robots.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 | -------------------------------------------------------------------------------- /launch/tb3_0.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | -------------------------------------------------------------------------------- /launch/tb3_1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | -------------------------------------------------------------------------------- /launch/two_tb_exploration.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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multi_robot_exploration 4 | 1.0.1 5 | Use Frontier Exploration on two turtlebots to map a space and the multirobot_map_merge to combine the local maps into one mergered map 6 | 7 | 8 | Kailey Smith 9 | 10 | 11 | 12 | MIT 13 | 14 | 15 | 16 | catkin 17 | move_base 18 | turtlebot3_navigation 19 | map_server 20 | robot_state_publisher 21 | xacro 22 | turtlebot3_gazebo 23 | turtlebot3_description 24 | gazebo_ros 25 | 26 | rviz 27 | joy 28 | teleop_twist_joy 29 | turtlebot3_slam 30 | turtlebot3_teleop 31 | turtlebot3_bringup 32 | slam_toolbox 33 | 34 | tf 35 | tf2_ros 36 | teleop_twist_keyboard 37 | multirobot_map_merge 38 | 39 | roscpp 40 | tf2_ros 41 | tf2_ros 42 | tf2 43 | tf2 44 | actionlib 45 | actionlib 46 | move_base_msgs 47 | move_base_msgs 48 | message_runtime 49 | message_runtime 50 | message_generation 51 | message_generation 52 | std_msgs 53 | std_msgs 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /src/map_expansion.cpp: -------------------------------------------------------------------------------- 1 | /// \file 2 | /// \brief This node reads in the robot's local SLAM map and creates a larger map to be used for the map_merge node 3 | /// 4 | /// PUBLISHES: 5 | /// /new_tb3_0_map (nav_msgs/OccupancyGrid): Publishes an expanded map with new width, height and origin 6 | /// /new_tb3_1_map (nav_msgs/OccupancyGrid): Publishes an expanded map new width, height and origin 7 | /// SUBSCRIBES: 8 | /// /tb3_0/map (nav_msgs/OccupancyGrid): Reads the map created by SLAM 9 | /// /tb3_1/map (nav_msgs/OccupancyGrid): Reads the map created by SLAM 10 | 11 | #include "ros/ros.h" 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // Define global message 18 | nav_msgs::OccupancyGrid slam0_map, slam1_map; 19 | 20 | /// \brief Reads the map data published from slam_toolbox 21 | /// \param msg - map message 22 | /// \returns nothing 23 | void map0Callback(const nav_msgs::OccupancyGrid::ConstPtr & msg) 24 | { 25 | slam0_map.header = msg->header; 26 | slam0_map.info = msg->info; 27 | slam0_map.data = msg->data; 28 | std::cout << "Got to map0 callback" << std::endl; 29 | } 30 | 31 | /// \brief Reads the map data published from slam_toolbox 32 | /// \param msg - map message 33 | /// \returns nothing 34 | void map1Callback(const nav_msgs::OccupancyGrid::ConstPtr & msg) 35 | { 36 | slam1_map.header = msg->header; 37 | slam1_map.info = msg->info; 38 | slam1_map.data = msg->data; 39 | } 40 | 41 | int main(int argc, char * argv[]) 42 | { 43 | ros::init(argc, argv, "map_expansion_node"); 44 | ros::NodeHandle nh; 45 | 46 | // Create the publisher and subscriber 47 | const auto new_tb3_0_map_pub = nh.advertise("new_tb3_0_map", 100); 48 | const auto map_meta0_sub = nh.subscribe("tb3_0/map", 100, map0Callback); 49 | 50 | const auto new_tb3_1_map_pub = nh.advertise("new_tb3_1_map", 100); 51 | const auto map_meta1_sub = nh.subscribe("tb3_1/map", 100, map1Callback); 52 | 53 | ros::Rate loop_rate(100); 54 | 55 | while (ros::ok()) 56 | { 57 | if ( (slam0_map.data.size() != 0) || (slam1_map.data.size() != 0) || slam0_map.info.origin.position.x !=0 || slam1_map.info.origin.position.x !=0) 58 | { 59 | // Create the new larger maps for each robot 60 | nav_msgs::OccupancyGrid new_tb3_0_map, new_tb3_1_map; 61 | new_tb3_0_map.header.frame_id = "new_tb3_0_map"; 62 | new_tb3_0_map.info.resolution = 0.05; 63 | new_tb3_0_map.info.origin.position.x = -10.0; 64 | new_tb3_0_map.info.origin.position.y = -10.0; 65 | new_tb3_0_map.info.origin.position.z = 0.0; 66 | new_tb3_0_map.info.origin.orientation.w = 0.0; 67 | 68 | new_tb3_1_map.header.frame_id = "new_tb3_1_map"; 69 | new_tb3_1_map.info.resolution = 0.05; 70 | new_tb3_1_map.info.origin.position.x = new_tb3_0_map.info.origin.position.x; 71 | new_tb3_1_map.info.origin.position.y = new_tb3_0_map.info.origin.position.y; 72 | new_tb3_1_map.info.origin.position.z = new_tb3_0_map.info.origin.position.z; 73 | new_tb3_1_map.info.origin.orientation.w = new_tb3_0_map.info.origin.orientation.w; 74 | 75 | // Set the new map width and heights 76 | const size_t width_ = 384; 77 | const size_t height_ = 384; 78 | new_tb3_0_map.info.width = width_; 79 | new_tb3_0_map.info.height = height_; 80 | new_tb3_1_map.info.width = width_; 81 | new_tb3_1_map.info.height = height_; 82 | 83 | // Determine how much space to fill in with unknown cells bewteen bottom of the new sized map and the original slam map 84 | const size_t bottom0_width_ = (slam0_map.info.origin.position.x - new_tb3_0_map.info.origin.position.x) / new_tb3_0_map.info.resolution; 85 | const size_t bottom1_width_ = (slam1_map.info.origin.position.x - new_tb3_1_map.info.origin.position.x) / new_tb3_1_map.info.resolution; 86 | 87 | const size_t bottom0_height_ = (slam0_map.info.origin.position.y - new_tb3_0_map.info.origin.position.y) / new_tb3_0_map.info.resolution; 88 | const size_t bottom1_height_ = (slam1_map.info.origin.position.y - new_tb3_1_map.info.origin.position.y) / new_tb3_1_map.info.resolution; 89 | 90 | ///////////////////////////////////////////////////////////////////////////// 91 | // For the frist turtlebot (tb3_0) 92 | //////////////////////////////////////////////////////////////////////////// 93 | // Map starts loading in from origin, which is the bottom right corner (for the deault orientation in rviz) 94 | // From the origin, the row components corresponds with width (+ x-dir which is up and + y-dir is to the left) 95 | // Fill in the all new cells in the new map with unknowns (-1) 96 | 97 | int c0 = 0; // start a counter for map0 98 | 99 | // Fill in the space between start of the new map to the start of the local SLAM map with -1s 100 | // (for the default orientation in Rviz, this is the space to the right of the SLAM map) 101 | for (int i=0; i < new_tb3_0_map.info.width * bottom0_height_; i++) 102 | { 103 | new_tb3_0_map.data.push_back(-1); 104 | } 105 | 106 | // Fill in the spaces on either side of the local SLAM map with -1s, but dont replace the current values from the local SLAM map 107 | for (int item_counter=0; item_counter < slam0_map.info.height; item_counter++) 108 | { 109 | // For all new cells between the new starting width and the original SLAM starting width, fill with -1s 110 | // (for the default orientation in Rviz, this is the space below the SLAM map) 111 | for (int q=0; q < bottom0_width_; q++) 112 | { 113 | new_tb3_0_map.data.push_back(-1); 114 | } 115 | 116 | // Fill in the current SLAM map information, in its initial location 117 | for (int a = 0; a < slam0_map.info.width; a++) 118 | { 119 | new_tb3_0_map.data.push_back(slam0_map.data[c0]); 120 | c0++; 121 | } 122 | 123 | // For all new cells between the new ending width and the original SLAM end width, fill with -1s 124 | // (for the default orientation in Rviz, this is the space above the SLAM map) 125 | for (int u=0; u < (new_tb3_0_map.info.width - slam0_map.info.width - bottom0_width_); u++) 126 | { 127 | new_tb3_0_map.data.push_back(-1); 128 | } 129 | } 130 | 131 | // Fill in the space between the end of the original SLAM map to the end of the new map with -1s 132 | // (for the default orientation in Rviz, this is the space to the left of the SLAM map) 133 | for (int z=0; z < ((height_ - slam0_map.info.height - bottom0_height_) * new_tb3_0_map.info.width); z++) 134 | { 135 | new_tb3_0_map.data.push_back(-1); 136 | } 137 | 138 | ///////////////////////////////////////////////////////////////////////////// 139 | // For the second turtlebot (tb3_1) 140 | //////////////////////////////////////////////////////////////////////////// 141 | // Map starts loading in from origin, which is the bottom right corner (for the deault orientation in rviz) 142 | // From the origin, the row components corresponds with width (+ x-dir which is up and + y-dir is to the left) 143 | // Fill in the all new cells in the new map with unknowns (-1) 144 | 145 | int c1 = 0; // start a counter for map 2 146 | 147 | // Fill in the space between start of the new map to the start of the local SLAM map with -1s 148 | // (for the default orientation in Rviz, this is the space to the right of the SLAM map) 149 | for (int i=0; i < new_tb3_1_map.info.width * bottom1_height_; i++) 150 | { 151 | new_tb3_1_map.data.push_back(-1); 152 | } 153 | 154 | // Fill in the spaces on either side of the local SLAM map with -1s, but dont replace the current values from the local SLAM map 155 | for (int item_counter=0; item_counter < slam1_map.info.height; item_counter++) 156 | { 157 | // For all new cells between the new starting width and the original SLAM starting width, fill with -1s 158 | // (for the default orientation in Rviz, this is the space below the SLAM map) 159 | for (int q=0; q < bottom1_width_; q++) 160 | { 161 | new_tb3_1_map.data.push_back(-1); 162 | } 163 | 164 | // Fill in the current SLAM map information, in its initial location 165 | for (int a = 0; a < slam1_map.info.width; a++) 166 | { 167 | new_tb3_1_map.data.push_back(slam1_map.data[c1]); 168 | c1++; 169 | } 170 | 171 | // For all new cells between the new ending width and the original SLAM end width, fill with -1s 172 | // (for the default orientation in Rviz, this is the space above the SLAM map) 173 | for (int u=0; u < (new_tb3_1_map.info.width - slam1_map.info.width - bottom1_width_); u++) 174 | { 175 | new_tb3_1_map.data.push_back(-1); 176 | } 177 | } 178 | 179 | // Fill in the space between the end of the original SLAM map to the end of the new map with -1s 180 | // (for the default orientation in Rviz, this is the space to the left of the SLAM map) 181 | for (int z=0; z < ((height_ - slam1_map.info.height - bottom1_height_) * new_tb3_1_map.info.width); z++) 182 | { 183 | new_tb3_1_map.data.push_back(-1); 184 | } 185 | 186 | // Publish the new maps 187 | new_tb3_0_map_pub.publish(new_tb3_0_map); 188 | new_tb3_1_map_pub.publish(new_tb3_1_map); 189 | } 190 | 191 | ros::spinOnce(); 192 | loop_rate.sleep(); 193 | } 194 | 195 | return 0; 196 | } -------------------------------------------------------------------------------- /src/single_tb_FE.cpp: -------------------------------------------------------------------------------- 1 | /// \file 2 | /// \brief With this node a turtlebot will explore a given space using Froniter Exploration 3 | 4 | /// PUBLISHES: 5 | /// /edges_map (nav_msgs/OccupancyGrid): Publishes the same map as SLAM, with visualization of the frontier edges 6 | /// SUBSCRIBES: 7 | /// /map (nav_msgs/OccupancyGrid): Reads the map created by SLAM to determine frontiers 8 | 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 | class FrontExpl 24 | { 25 | public: 26 | FrontExpl() 27 | { 28 | FE_map_pub = nh.advertise("edges_map", 1); 29 | map_sub = nh.subscribe("map", 10, &FrontExpl::mapCallback, this); 30 | std::cout << "Initialized the publisher and subcriber" << std::endl; 31 | } 32 | 33 | /// \brief Reads the map data published from slam_toolbox 34 | /// \param msg - map message 35 | /// \returns nothing 36 | void mapCallback(const nav_msgs::OccupancyGrid & msg) 37 | { 38 | FE_map = msg; 39 | // region_map = msg; 40 | 41 | FE_map.header.frame_id = "edges_map"; 42 | // region_map.header.frame_id = "region_map"; 43 | 44 | std::cout << "Got to map callback" << std::endl; 45 | } 46 | 47 | /// \brief Stores a vector of index values of the 8 cell neighborhood relative to the input cell 48 | /// \param cell - map cell 49 | /// \returns nothing 50 | void neighborhood(int cell) 51 | { 52 | // Clear any previous values in the vectors 53 | neighbor_index.clear(); 54 | neighbor_value.clear(); 55 | 56 | // Store neighboring values and indexes 57 | neighbor_index.push_back(cell - map_width - 1); 58 | neighbor_index.push_back(cell - map_width); 59 | neighbor_index.push_back(cell - map_width + 1); 60 | neighbor_index.push_back(cell-1); 61 | neighbor_index.push_back(cell+1); 62 | neighbor_index.push_back(cell + map_width - 1); 63 | neighbor_index.push_back(cell + map_width); 64 | neighbor_index.push_back(cell + map_width + 1); 65 | 66 | sort( neighbor_index.begin(), neighbor_index.end() ); 67 | neighbor_index.erase( unique( neighbor_index.begin(), neighbor_index.end() ), neighbor_index.end() ); 68 | } 69 | 70 | /// \brief Stores a vector of frontier edges 71 | /// \returns nothing 72 | void find_all_edges() 73 | { 74 | std::cout << "Finding all the frontier edges" << std::endl; 75 | 76 | // Starting one row up and on space in on the map so there are no indexing issues 77 | for (double x = map_width + 1; x < (map_width * map_height) - map_width - 1; x++) 78 | { 79 | // For all cells in the map, check if a cell is unknown 80 | if (FE_map.data.at(x) == -1) 81 | { 82 | // If there is an unknown cell, then check neighboring cells to find a potential frontier edge (free cell) 83 | neighborhood(x); 84 | 85 | for(int i = 0; i < neighbor_index.size(); i++) // For all neighboring cells 86 | { 87 | if (FE_map.data.at(neighbor_index.at(i)) == 0) // If one of the neighboring cells is free 88 | { 89 | // If one of the neighboring cells is free, store it in the edges vector 90 | FE_map.data.at(neighbor_index.at(i)) = 10; // Visualize the frontier edge cells 91 | edge_vec.push_back(neighbor_index.at(i)); 92 | 93 | } 94 | } 95 | 96 | } 97 | } 98 | } 99 | 100 | /// \brief Compares two cells to see if the are identical or unique 101 | /// \param - curr_cell: The current cell being evaluated 102 | /// \param - next_cell: The next cell to be evaluated 103 | /// \returns true or false 104 | bool check_edges(int curr_cell, int next_cell) 105 | { 106 | if (curr_cell == next_cell) 107 | { 108 | return false; 109 | } 110 | else 111 | { 112 | return true; 113 | } 114 | } 115 | 116 | /// \brief Given the frontier edges, group the neighboring edges into regions 117 | /// \returns nothing 118 | void find_regions() 119 | { 120 | std::cout << "Finding regions for tb3" << std::endl; 121 | 122 | for (int q = 0; q < edge_vec.size() - 1; q++) 123 | { 124 | // For each frontier edge, check that the next value is unique and not a repeat 125 | unique_flag = check_edges(edge_vec.at(q), edge_vec.at(q+1)); 126 | 127 | if (unique_flag == true) 128 | { 129 | // If we have an original frontier edge, check the neighboring cells 130 | neighborhood(edge_vec.at(q)); 131 | 132 | for(int i = 0; i < neighbor_index.size(); i++) 133 | { 134 | // For all the cells nighrboring the frontier edge, check to see if there is another frontier edge 135 | if (neighbor_index.at(i) == edge_vec.at(q+1)) 136 | { 137 | // If a frontier edge is in the neightborhood of another frontier edge, add it to a region 138 | edge_index = edge_vec.at(q); 139 | temp_group.push_back(edge_vec.at(q)); 140 | 141 | sort( temp_group.begin(), temp_group.end() ); 142 | temp_group .erase( unique( temp_group.begin(), temp_group.end() ), temp_group.end() ); 143 | 144 | // Increase the counter to keep track of the region's size 145 | group_c++; 146 | } 147 | } 148 | 149 | if (group_c == prev_group_c) // If we didnt any any edeges to our region, region is complete 150 | { 151 | if (group_c < 5) // frontier region too small 152 | { 153 | // If the forntier region is smaller than 5 cells, dont use it 154 | } 155 | 156 | else 157 | { 158 | // If the frontier region is larger than 5 cells, find the regions centroid 159 | centroid = (temp_group.size()) / 2; 160 | centroid_index = temp_group.at(centroid); 161 | centroids.push_back(centroid_index); 162 | } 163 | 164 | // Reset the region vector and size counter 165 | group_c = 0; 166 | temp_group.clear(); 167 | } 168 | 169 | else 170 | { 171 | // If we found a frontier edge, increase the group size 172 | prev_group_c = group_c; 173 | } 174 | } 175 | 176 | else 177 | { 178 | // If we got a duplicate cell, do nothing 179 | } 180 | } 181 | } 182 | 183 | /// \brief Finds the transform between the map frame and the robot's base_footprint frame 184 | /// \returns nothing 185 | void find_transform() 186 | { 187 | // Find the robot's current pose via the transform from the map frame and the base_footprint 188 | transformS = tfBuffer.lookupTransform(map_frame, body_frame, ros::Time(0), ros::Duration(3.0)); 189 | 190 | transformS.header.stamp = ros::Time(); 191 | transformS.header.frame_id = body_frame; 192 | transformS.child_frame_id = map_frame; 193 | 194 | robot_pose_.position.x = transformS.transform.translation.x; 195 | robot_pose_.position.y = transformS.transform.translation.y; 196 | robot_pose_.position.z = 0.0; 197 | robot_pose_.orientation = transformS.transform.rotation; 198 | 199 | std::cout << "Robot pose is " << robot_pose_.position.x << " , " << robot_pose_.position.y << std::endl; 200 | } 201 | 202 | /// \brief Given a centroid cell, convert the centroid to x-y coordinates in the map frame and determine its distance from the robot 203 | /// \returns nothing 204 | void centroid_index_to_point() 205 | { 206 | for (int t = 0; t < centroids.size(); t++) 207 | { 208 | // For all the centorid cells, find the x and y coordinates in the map frame 209 | point.x = (centroids.at(t) % FE_map.info.width)*FE_map.info.resolution + FE_map.info.origin.position.x; 210 | point.y = floor(centroids.at(t) / FE_map.info.width)*FE_map.info.resolution + FE_map.info.origin.position.y; 211 | 212 | for (int w = 0; w < prev_cent_x.size(); w++) 213 | { 214 | // Compare the previous centroids to the current calculated centroid 215 | if ( (fabs( prev_cent_x.at(w) - point.x) < 0.01) && (fabs( prev_cent_y.at(w) - point.y) < 0.01) ) 216 | { 217 | // If the current centroid is too close to a previous centroid, skip 218 | std::cout << "Already went to this centroid " << prev_cent_x.at(w) << " , " << prev_cent_y.at(w) << std::endl; 219 | goto bad_centroid; 220 | } 221 | } 222 | 223 | if((point.x < FE_map.info.origin.position.x + 0.05) && (point.y < FE_map.info.origin.position.y + 0.05)) 224 | { 225 | // If the centroid is too close to the map orgin, skip 226 | goto bad_centroid; 227 | } 228 | 229 | else 230 | { 231 | // If the centroid is valid, add its x and y values to their respective vectors 232 | centroid_Xpts.push_back(point.x); 233 | centroid_Ypts.push_back(point.y); 234 | 235 | // Determine the distance between the current centroid and the robot's position 236 | double delta_x = point.x - robot_pose_.position.x; 237 | double delta_y = point.y - robot_pose_.position.y; 238 | double sum = (pow(delta_x ,2)) + (pow(delta_y ,2)); 239 | dist = pow( sum , 0.5 ); 240 | 241 | // Store the distance value in a vector 242 | dist_arr.push_back(dist); 243 | } 244 | 245 | // Skip to the end of the for loop if there was an invalid centroid 246 | bad_centroid: 247 | std::cout << "Ignore bad centroid" << std::endl; 248 | } 249 | } 250 | 251 | /// \brief Given all the centroid distance values, find the closest centroid to move to 252 | /// \returns nothing 253 | void find_closest_centroid() 254 | { 255 | // Set the first smallest distance to be large 256 | smallest_dist = 9999999.0; 257 | 258 | for(int u = 0; u < dist_arr.size(); u++) 259 | { 260 | // For each centroid distance, determine if the current centroid is closer than the previous closest centroid 261 | 262 | if (dist_arr.at(u) < 0.1) 263 | { 264 | // If the centroid distance is less than 0.1m, its too close. Ignore it 265 | } 266 | 267 | else if (dist_arr.at(u) < smallest_dist) 268 | { 269 | // If the current distance element is smaller than the previous closest centroid 270 | // replace the smallest distance variable with the current distance 271 | smallest_dist = dist_arr.at(u); 272 | 273 | // Update the centroid that the robot will move to 274 | move_to_pt = u; 275 | } 276 | 277 | else 278 | { 279 | // If the current distance value is larger than 0.1 and the smallest_dist value, then ignore it 280 | } 281 | } 282 | } 283 | 284 | /// \brief Given the frontier edges, convert the cell to x-y coordinates in the map frame and determine its distance from the robot 285 | /// \returns nothing 286 | void edge_index_to_point() 287 | { 288 | for (int t = 0; t < edge_vec.size(); t++) 289 | { 290 | // For all the frontier edge cells, find the x and y coordinates in the map frame 291 | point.x = (edge_vec.at(t) % FE_map.info.width)*FE_map.info.resolution + FE_map.info.origin.position.x; 292 | point.y = floor(edge_vec.at(t) / FE_map.info.width)*FE_map.info.resolution + FE_map.info.origin.position.y; 293 | 294 | // Add the cells x and y values to their respective vectors 295 | centroid_Xpts.push_back(point.x); 296 | centroid_Ypts.push_back(point.y); 297 | 298 | // Determine the distance between the current frontier edge and the robot's position 299 | double delta_x = point.x - robot_pose_.position.x; 300 | double delta_y = point.y - robot_pose_.position.y; 301 | double sum = (pow(delta_x ,2)) + (pow(delta_y ,2)); 302 | dist = pow( sum , 0.5 ); 303 | 304 | // Store the distance value in a vector 305 | dist_arr.push_back(dist); 306 | } 307 | } 308 | 309 | /// \brief Calls all other functions to find frontier edges, regions and a goal to move to. Then uses the action server to move to that goal 310 | /// \returns nothing 311 | void main_loop() 312 | { 313 | std::cout << "Entered the main loop" << std::endl; 314 | 315 | typedef actionlib::SimpleActionClient MoveBaseClient; 316 | MoveBaseClient ac("move_base", true); 317 | 318 | // Wait for the action server to come up 319 | while(!ac.waitForServer(ros::Duration(5.0))) 320 | { 321 | ROS_INFO("Waiting for the move_base action server to come up"); 322 | } 323 | ROS_INFO("Connected to move base server"); 324 | 325 | tf2_ros::TransformListener tfListener(tfBuffer); 326 | ros::Rate loop_rate(40); 327 | 328 | while (ros::ok()) 329 | { 330 | std::cout << "While ros okay " << std::endl; 331 | ros::spinOnce(); 332 | 333 | // If there is no map data, do nothing and instead start the loop over again 334 | if (FE_map.data.size()!=0) 335 | { 336 | map_width = FE_map.info.width; 337 | map_height = FE_map.info.height; 338 | 339 | // Find the frontier edges 340 | find_all_edges(); 341 | 342 | // If there are no frontier edges, skip to the end of the main_loop and try again 343 | if (edge_vec.size() == 0) 344 | { 345 | goto skip; 346 | } 347 | 348 | sort( edge_vec.begin(), edge_vec.end() ); 349 | edge_vec.erase( unique( edge_vec.begin(), edge_vec.end() ), edge_vec.end() ); 350 | 351 | FE_map_pub.publish(FE_map); 352 | 353 | // Given the forntier edges, find the frontier regions and their centroids 354 | find_regions(); 355 | 356 | // Find the transfrom between the map frame the base_footprint frame 357 | // in order to determine the robot's position 358 | find_transform(); 359 | 360 | // Given the centroid vector, convert the controids from map cells to x-y coordinates in the map frame 361 | // so a goal can be sent to move_base 362 | centroid_index_to_point(); 363 | 364 | // If there are no values in the centroid x or y vectors, find the closest frontier edge to move to 365 | if ( ( centroid_Xpts.size() == 0 ) || ( centroid_Ypts.size() == 0) ) 366 | { 367 | centroid_Xpts.clear(); 368 | centroid_Ypts.clear(); 369 | dist_arr.clear(); 370 | std::cout << "Couldnt find a centroid, move to closest edge instead" << std::endl; 371 | 372 | // Given the edge vector, convert the edges from map cells to x-y coordinates in the map frame 373 | // so a goal can be sent to move_base 374 | edge_index_to_point(); 375 | } 376 | 377 | // Of all the centroids, determine which centroid is the closest 378 | // Choose the closest centroid to move to 379 | find_closest_centroid(); 380 | 381 | std::cout << "Moving to centroid " << centroid_Xpts.at(move_to_pt) << " , " << centroid_Ypts.at(move_to_pt) << std::endl; 382 | std::cout << "At a distance of " << smallest_dist << std::endl; 383 | 384 | // Add the current centroid to the vector of previously visited centroids 385 | prev_cent_x.push_back(centroid_Xpts.at(move_to_pt)); 386 | prev_cent_y.push_back(centroid_Ypts.at(move_to_pt)); 387 | 388 | // Determine the move_base goal 389 | goal.target_pose.header.frame_id = "map"; 390 | goal.target_pose.header.stamp = ros::Time(); 391 | goal.target_pose.pose.position.x = centroid_Xpts.at(move_to_pt); 392 | goal.target_pose.pose.position.y = centroid_Ypts.at(move_to_pt); 393 | goal.target_pose.pose.orientation.w = 1.0; 394 | 395 | // Send the goal to move_base 396 | ROS_INFO("Sending goal"); 397 | ac.sendGoal(goal); 398 | 399 | // Wait for the action to return 400 | ac.waitForResult(ros::Duration(60)); 401 | 402 | if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 403 | { 404 | ROS_INFO("You have reached the goal!"); 405 | } 406 | else 407 | { 408 | ROS_INFO("The base failed for some reason"); 409 | } 410 | 411 | std::cout << "" << std::endl; 412 | } 413 | 414 | // If the size of the map data is 0, then run through the loop again 415 | else 416 | { 417 | ROS_INFO("Waiting for first map"); 418 | } 419 | 420 | // Skip to the end of the loop if there are no fontier regions 421 | skip: 422 | std::cout << "Starting loop over" << std::endl; 423 | sleep(1.0); 424 | 425 | std::cout << "Resetting vairbales and clearing all vectors" << std::endl; 426 | centroid = 0; 427 | centroid_index = 0; 428 | dist_arr.clear(); 429 | edge_vec.clear(); 430 | neighbor_index.clear(); 431 | neighbor_value.clear(); 432 | centroids.clear(); 433 | centroid_Xpts.clear(); 434 | centroid_Ypts.clear(); 435 | 436 | ros::spinOnce(); 437 | 438 | std::cout << "Spinning like a ballerina" << std::endl; 439 | 440 | loop_rate.sleep(); 441 | 442 | std::cout << "Asleep and done with main_loop" << std::endl; 443 | } 444 | } 445 | 446 | private: 447 | ros::NodeHandle nh; 448 | ros::Publisher FE_map_pub; 449 | ros::Subscriber map_sub; 450 | tf2_ros::Buffer tfBuffer; 451 | nav_msgs::OccupancyGrid FE_map; 452 | geometry_msgs::Pose robot_pose_; 453 | geometry_msgs::Point point; 454 | geometry_msgs::TransformStamped transformS; 455 | move_base_msgs::MoveBaseGoal goal; 456 | typedef actionlib::SimpleActionClient MoveBaseClient; 457 | std::string map_frame = "map"; 458 | std::string body_frame = "base_footprint"; 459 | std::vector edge_vec, neighbor_index, neighbor_value; 460 | std::vector centroids, temp_group; 461 | std::vector centroid_Xpts, centroid_Ypts, dist_arr, prev_cent_x, prev_cent_y; 462 | int group_c=0, prev_group_c=0, centroid=0, centroid_index=0, move_to_pt=0, map_width=0, map_height=0, mark_edge=0, edge_index=0; 463 | double smallest_dist = 9999999.0, dist = 0.0; 464 | bool unique_flag = true; 465 | }; 466 | 467 | int main(int argc, char * argv[]) 468 | { 469 | ros::init(argc, argv, "tb3_FE_node"); 470 | FrontExpl FE; 471 | FE.main_loop(); 472 | ros::spin(); 473 | 474 | return 0; 475 | } -------------------------------------------------------------------------------- /src/tb3_0_FE.cpp: -------------------------------------------------------------------------------- 1 | /// \file 2 | /// \brief This node causes the robot to move autonomously via Frontier Exploration 3 | 4 | /// PUBLISHES: 5 | /// /edges_map_0 (nav_msgs/OccupancyGrid): Publishes the same map as SLAM, with visualization of the frontier edges 6 | /// SUBSCRIBES: 7 | /// /map (nav_msgs/OccupancyGrid): Reads the map created by SLAM to determine frontiers 8 | /// SERVICES: 9 | /// /tb3_0_start (bool): Starts the robot's frontier exploraiton algorithm 10 | 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 | #include 26 | 27 | 28 | class FrontExpl 29 | { 30 | public: 31 | FrontExpl() 32 | { 33 | FE0_map_pub = nh.advertise("edges_map_0", 1); 34 | map_0_sub = nh.subscribe("map", 10, &FrontExpl::mapCallback, this); 35 | start0_srv = nh.advertiseService("tb3_0_start", &FrontExpl::startCallback, this); 36 | std::cout << "Initialized tb3_0 publishers, subcribers and service" << std::endl; 37 | } 38 | 39 | /// \brief Starts frontier exploration 40 | /// \param req - service request 41 | /// \param res - service response 42 | /// \returns true when done 43 | bool startCallback(multi_robot_exploration::tb3_0_start::Request &req, multi_robot_exploration::tb3_0_start::Response &res) 44 | { 45 | std::cout << "Got to start tb3_0 service" << std::endl; 46 | start0_flag = true; 47 | return true; 48 | } 49 | 50 | /// \brief Reads the map data published from slam_toolbox 51 | /// \param msg - map message 52 | /// \returns nothing 53 | void mapCallback(const nav_msgs::OccupancyGrid & msg) 54 | { 55 | FE0_map = msg; 56 | FE0_map.header.frame_id = "edges_map_0"; 57 | std::cout << "Got to map0 callback" << std::endl; 58 | } 59 | 60 | /// \brief Stores a vector of index values of the 8 cell neighborhood relative to the input cell 61 | /// \param cell - map cell 62 | /// \returns nothing 63 | void neighborhood(int cell) 64 | { 65 | // Clear any previous values in the vectors 66 | neighbor0_index.clear(); 67 | neighbor0_value.clear(); 68 | 69 | // Store neighboring values and indexes 70 | neighbor0_index.push_back(cell - map_width - 1); 71 | neighbor0_index.push_back(cell - map_width); 72 | neighbor0_index.push_back(cell - map_width + 1); 73 | neighbor0_index.push_back(cell-1); 74 | neighbor0_index.push_back(cell+1); 75 | neighbor0_index.push_back(cell + map_width - 1); 76 | neighbor0_index.push_back(cell + map_width); 77 | neighbor0_index.push_back(cell + map_width + 1); 78 | 79 | sort( neighbor0_index.begin(), neighbor0_index.end() ); 80 | neighbor0_index.erase( unique( neighbor0_index.begin(), neighbor0_index.end() ), neighbor0_index.end() ); 81 | } 82 | 83 | /// \brief Stores a vector of frontier edges 84 | /// \returns nothing 85 | void find_all_edges() 86 | { 87 | std::cout << "Finding all the tb3_0 edges" << std::endl; 88 | 89 | // Starting one row up and on space in on the map so there are no indexing issues 90 | for (double x = map_width + 1; x < (map_width * map_height) - map_width - 1; x++) 91 | { 92 | // For all cells in the map, check if a cell is unknown 93 | if (FE0_map.data.at(x) == -1) 94 | { 95 | // If there is an unknown cell, then check neighboring cells to find a potential frontier edge (free cell) 96 | neighborhood(x); 97 | 98 | for(int i = 0; i < neighbor0_index.size(); i++) // For all neighboring cells 99 | { 100 | if (FE0_map.data.at(neighbor0_index.at(i)) == 0) 101 | { 102 | // If one of the neighboring cells is free, store it in the edges vector 103 | FE0_map.data.at(neighbor0_index.at(i)) = 10; // Visualize the frontier edge cells 104 | edge0_vec.push_back(neighbor0_index.at(i)); 105 | 106 | } 107 | } 108 | 109 | } 110 | } 111 | 112 | } 113 | 114 | /// \brief Compares two cells to see if the are identical or unique 115 | /// \param - curr_cell: The current cell being evaluated 116 | /// \param - next_cell: The next cell to be evaluated 117 | /// \returns true or false 118 | bool check_edges(int curr_cell, int next_cell) 119 | { 120 | if (curr_cell == next_cell) 121 | { 122 | return false; 123 | } 124 | else 125 | { 126 | return true; 127 | } 128 | } 129 | 130 | /// \brief Given the frontier edges, group the neighboring edges into regions 131 | /// \returns nothing 132 | void find_regions() 133 | { 134 | std::cout << "Finding regions for tb3_0" << std::endl; 135 | 136 | for (int q = 0; q < edge0_vec.size() - 1; q++) 137 | { 138 | // For each frontier edge, check that the next value is unique and not a repeat 139 | unique_flag = check_edges(edge0_vec.at(q), edge0_vec.at(q+1)); 140 | 141 | if (unique_flag == true) 142 | { 143 | // If we have an original frontier edge, check the neighboring cells 144 | neighborhood(edge0_vec.at(q)); 145 | 146 | for(int i = 0; i < neighbor0_index.size(); i++) 147 | { 148 | // For all the cells nighrboring the frontier edge, check to see if there is another frontier edge 149 | if (neighbor0_index.at(i) == edge0_vec.at(q+1)) 150 | { 151 | // If a frontier edge is in the neightborhood of another frontier edge, add it to a region 152 | edge_index = edge0_vec.at(q); 153 | temp_group0.push_back(edge0_vec.at(q));; 154 | 155 | sort( temp_group0.begin(), temp_group0.end() ); 156 | temp_group0 .erase( unique( temp_group0.begin(), temp_group0.end() ), temp_group0.end() ); 157 | 158 | // Increase the counter to keep track of the region's size 159 | group0_c++; 160 | } 161 | } 162 | 163 | if (group0_c == prev_group0_c) // If we didnt any any edeges to our region, region is complete 164 | { 165 | if (group0_c < 5) // frontier region too small 166 | { 167 | // If the forntier region is smaller than 5 cells, dont use it 168 | } 169 | 170 | else 171 | { 172 | // If the frontier region is larger than 5 cells, find the regions centroid 173 | centroid0 = (temp_group0.size()) / 2; 174 | centroid0_index = temp_group0.at(centroid0); 175 | centroids0.push_back(centroid0_index); 176 | } 177 | 178 | // Reset the region vector and size counter 179 | group0_c = 0; 180 | temp_group0.clear(); 181 | } 182 | 183 | else 184 | { 185 | // If we found a frontier edge, increase the group size 186 | prev_group0_c = group0_c; 187 | } 188 | } 189 | 190 | else 191 | { 192 | // If we got a duplicate cell, do nothing 193 | } 194 | 195 | } 196 | 197 | } 198 | 199 | /// \brief Finds the transform between the map frame and the robot's base_footprint frame 200 | /// \returns nothing 201 | void find_transform() 202 | { 203 | // Find current location and move to the nearest centroid 204 | // Get robot pose 205 | transformS = tfBuffer.lookupTransform(map0_frame, body0_frame, ros::Time(0), ros::Duration(3.0)); 206 | 207 | transformS.header.stamp = ros::Time(); 208 | transformS.header.frame_id = body0_frame; 209 | transformS.child_frame_id = map0_frame; 210 | 211 | robot0_pose_.position.x = transformS.transform.translation.x; 212 | robot0_pose_.position.y = transformS.transform.translation.y; 213 | robot0_pose_.position.z = 0.0; 214 | robot0_pose_.orientation = transformS.transform.rotation; 215 | // robot0_pose_.orientation.y = transformS.transform.rotation.y; 216 | // robot0_pose_.orientation.z = transformS.transform.rotation.z; 217 | // robot0_pose_.orientation.w = transformS.transform.rotation.w; 218 | 219 | std::cout << "Robot pose is " << robot0_pose_.position.x << " , " << robot0_pose_.position.y << std::endl; 220 | } 221 | 222 | /// \brief Given a centroid cell, convert the centroid to x-y coordinates in the map frame and determine its distance from the robot 223 | /// \returns nothing 224 | void centroid_index_to_point() 225 | { 226 | for (int t = 0; t < centroids0.size(); t++) 227 | { 228 | // For all the centorid cells, find the x and y coordinates in the map frame 229 | point.x = (centroids0.at(t) % FE0_map.info.width)*FE0_map.info.resolution + FE0_map.info.origin.position.x; 230 | point.y = floor(centroids0.at(t) / FE0_map.info.width)*FE0_map.info.resolution + FE0_map.info.origin.position.y; 231 | 232 | for (int w = 0; w < prev_cent_0x.size(); w++) 233 | { 234 | // Compare the previous centroids to the current calculated centroid 235 | if ( (fabs( prev_cent_0x.at(w) - point.x) < 0.01) && (fabs( prev_cent_0y.at(w) - point.y) < 0.01) ) 236 | { 237 | // If the current centroid is too close to a previous centroid, skip 238 | std::cout << "Already went to this centroid " << prev_cent_0x.at(w) << " , " << prev_cent_0y.at(w) << std::endl; 239 | goto bad_centroid; 240 | } 241 | } 242 | 243 | if((point.x < FE0_map.info.origin.position.x + 0.05) && (point.y < FE0_map.info.origin.position.y + 0.05)) 244 | { 245 | // If the centroid is too close to the map orgin, skip 246 | goto bad_centroid; 247 | } 248 | 249 | else 250 | { 251 | // If the centroid is valid, add its x and y values to their respective vectors 252 | centroid0_Xpts.push_back(point.x); 253 | centroid0_Ypts.push_back(point.y); 254 | 255 | // Determine the distance between the current centroid and the robot's position 256 | double delta_x = point.x - robot0_pose_.position.x; 257 | double delta_y = point.y - robot0_pose_.position.y; 258 | double sum = (pow(delta_x ,2)) + (pow(delta_y ,2)); 259 | dist0 = pow( sum , 0.5 ); 260 | 261 | // Store the distance value in a vector 262 | dist0_arr.push_back(dist0); 263 | } 264 | 265 | // Skip to the end of the for loop if there was an invalid centroid 266 | bad_centroid: 267 | std::cout << "Ignore bad centroid" << std::endl; 268 | } 269 | } 270 | 271 | /// \brief Given all the centroid distance values, find the closest centroid to move to 272 | /// \returns nothing 273 | void find_closest_centroid() 274 | { 275 | // Set the first smallest distance to be large 276 | smallest = 9999999.0; 277 | for(int u = 0; u < dist0_arr.size(); u++) 278 | { 279 | // For each centroid distance, determine if the current centroid is closer than the previous closest centroid 280 | 281 | if (dist0_arr.at(u) < 0.1) 282 | { 283 | // If the centroid distance is less than 0.1m, its too close. Ignore it 284 | } 285 | else if (dist0_arr.at(u) < smallest) 286 | { 287 | // If the current distance element is smaller than the previous closest centroid 288 | // replace the smallest distance variable with the current distance 289 | smallest = dist0_arr.at(u); 290 | 291 | // Update the centroid that the robot will move to 292 | move_to_pt = u; 293 | } 294 | 295 | else 296 | { 297 | // If the current distance value is larger than 0.1 and the smallest_dist value, then ignore it 298 | } 299 | } 300 | } 301 | 302 | /// \brief Given the frontier edges, convert the cell to x-y coordinates in the map frame and determine its distance from the robot 303 | /// \returns nothing 304 | void edge_index_to_point() 305 | { 306 | for (int t = 0; t < edge0_vec.size(); t++) 307 | { 308 | // For all the frontier edge cells, find the x and y coordinates in the map frame 309 | point.x = (edge0_vec.at(t) % FE0_map.info.width)*FE0_map.info.resolution + FE0_map.info.origin.position.x; 310 | point.y = floor(edge0_vec.at(t) / FE0_map.info.width)*FE0_map.info.resolution + FE0_map.info.origin.position.y; 311 | 312 | // Add the cells x and y values to their respective vectors 313 | centroid0_Xpts.push_back(point.x); 314 | centroid0_Ypts.push_back(point.y); 315 | 316 | // Determine the distance between the current frontier edge and the robot's position 317 | double delta_x = point.x - robot0_pose_.position.x; 318 | double delta_y = point.y - robot0_pose_.position.y; 319 | double sum = (pow(delta_x ,2)) + (pow(delta_y ,2)); 320 | dist0 = pow( sum , 0.5 ); 321 | 322 | // Store the distance value in a vector 323 | dist0_arr.push_back(dist0); 324 | } 325 | } 326 | 327 | /// \brief Calls all other functions to find frontier edges, regions and a goal to move to. Then uses the action server to move to that goal 328 | /// \returns nothing 329 | void main_loop() 330 | { 331 | std::cout << "Entered the main loop for 0" << std::endl; 332 | 333 | typedef actionlib::SimpleActionClient MoveBaseClient; 334 | MoveBaseClient ac("tb3_0/move_base", true); 335 | 336 | // Wait for the action server to come up 337 | while(!ac.waitForServer(ros::Duration(5.0))) 338 | { 339 | ROS_INFO("Waiting for the move_base action server to come up"); 340 | } 341 | ROS_INFO("Connected to move base server"); 342 | 343 | tf2_ros::TransformListener tfListener(tfBuffer); 344 | ros::Rate loop_rate(40); 345 | 346 | while (ros::ok()) 347 | { 348 | std::cout << "While ros okay for tb3_0" << std::endl; 349 | ros::spinOnce(); 350 | 351 | // If there is no map data and / or start service isnt called, do nothing and instead start the loop over again 352 | if (FE0_map.data.size()!=0 && start0_flag == true) 353 | { 354 | map_width = FE0_map.info.width; 355 | map_height = FE0_map.info.height; 356 | 357 | // Find the frontier edges 358 | find_all_edges(); 359 | 360 | // If there are no frontier edges, skip to the end of the main_loop and try again 361 | if (edge0_vec.size() == 0) 362 | { 363 | goto skip; 364 | } 365 | 366 | sort( edge0_vec.begin(), edge0_vec.end() ); 367 | edge0_vec.erase( unique( edge0_vec.begin(), edge0_vec.end() ), edge0_vec.end() ); 368 | 369 | FE0_map_pub.publish(FE0_map); 370 | // Given the forntier edges, find the frontier regions and their centroids 371 | find_regions(); 372 | 373 | // Find the transfrom between the map frame the base_footprint frame 374 | // in order to determine the robot's position 375 | find_transform(); 376 | 377 | // Given the centroid vector, convert the controids from map cells to x-y coordinates in the map frame 378 | // so a goal can be sent to move_base 379 | centroid_index_to_point(); 380 | 381 | // If there are no values in the centroid x or y vectors, find the closest frontier edge to move to 382 | if ( ( centroid0_Xpts.size() == 0 ) || ( centroid0_Ypts.size() == 0) ) 383 | { 384 | centroid0_Xpts.clear(); 385 | centroid0_Ypts.clear(); 386 | dist0_arr.clear(); 387 | std::cout << "Couldnt find a centroid, move to closest edge instead" << std::endl; 388 | 389 | // Given the edge vector, convert the edges from map cells to x-y coordinates in the map frame 390 | // so a goal can be sent to move_base 391 | edge_index_to_point(); 392 | } 393 | 394 | // Of all the centroids, determine which centroid is the closest 395 | // Choose the closest centroid to move to 396 | find_closest_centroid(); 397 | 398 | std::cout << "Moving to centroid " << centroid0_Xpts.at(move_to_pt) << " , " << centroid0_Ypts.at(move_to_pt) << std::endl; 399 | std::cout << "At a distance of " << smallest << std::endl; 400 | 401 | // Add the current centroid to the vector of previously visited centroids 402 | prev_cent_0x.push_back(centroid0_Xpts.at(move_to_pt)); 403 | prev_cent_0y.push_back(centroid0_Ypts.at(move_to_pt)); 404 | 405 | // Determine the move_base goal 406 | goal.target_pose.header.frame_id = "tb3_0/map"; // Needs to be AN ACTUAL FRAME 407 | // goal.target_pose.header.frame_id = "map"; // Needs to be AN ACTUAL FRAME 408 | goal.target_pose.header.stamp = ros::Time(); 409 | goal.target_pose.pose.position.x = centroid0_Xpts.at(move_to_pt); 410 | goal.target_pose.pose.position.y = centroid0_Ypts.at(move_to_pt); 411 | goal.target_pose.pose.orientation.w = 1.0; 412 | 413 | ROS_INFO("Sending goal"); 414 | ac.sendGoal(goal); 415 | 416 | // Wait for the action to return 417 | ac.waitForResult(ros::Duration(60)); 418 | 419 | if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 420 | { 421 | ROS_INFO("You have reached the goal!"); 422 | } 423 | else 424 | { 425 | ROS_INFO("The base failed for some reason"); 426 | } 427 | 428 | std::cout << "" << std::endl; 429 | } 430 | 431 | // If the size of the map data is 0, then run through the loop agai 432 | else 433 | { 434 | ROS_INFO("Waiting for first map"); 435 | } 436 | 437 | // Skip to the end of the loop if there are no fontier regions 438 | skip: 439 | std::cout << "Starting loop over" << std::endl; 440 | sleep(1.0); 441 | 442 | std::cout << "Resetting vairbales and clearing all vectors" << std::endl; 443 | centroid0 = 0; 444 | centroid0_index = 0; 445 | dist0_arr.clear(); 446 | edge0_vec.clear(); 447 | neighbor0_index.clear(); 448 | neighbor0_value.clear(); 449 | centroids0.clear(); 450 | centroid0_Xpts.clear(); 451 | centroid0_Ypts.clear(); 452 | 453 | ros::spinOnce(); 454 | 455 | std::cout << "Spinning like a ballerina" << std::endl; 456 | 457 | loop_rate.sleep(); 458 | 459 | std::cout << "Asleep and done with main_loop" << std::endl; 460 | } 461 | } 462 | 463 | private: 464 | ros::NodeHandle nh; 465 | ros::Publisher FE0_map_pub; 466 | ros::Subscriber map_0_sub; 467 | ros::ServiceServer start0_srv; 468 | tf2_ros::Buffer tfBuffer; 469 | nav_msgs::OccupancyGrid FE0_map; 470 | geometry_msgs::Pose robot0_pose_; 471 | geometry_msgs::Point point; 472 | geometry_msgs::TransformStamped transformS; 473 | move_base_msgs::MoveBaseGoal goal; 474 | typedef actionlib::SimpleActionClient MoveBaseClient; 475 | std::string map0_frame = "tb3_0/map"; 476 | std::string body0_frame = "tb3_0/base_footprint"; 477 | std::vector edge0_vec, neighbor0_index, neighbor0_value; 478 | std::vector centroids0, temp_group0; 479 | std::vector centroid0_Xpts, centroid0_Ypts, dist0_arr, prev_cent_0x, prev_cent_0y; 480 | int group0_c=0, prev_group0_c=0, centroid0=0, centroid0_index=0, move_to_pt=0, map_width=0, map_height=0, mark_edge=0, edge_index=0; 481 | double smallest = 9999999.0, dist0= 0.0; 482 | bool unique_flag = true; 483 | bool start0_flag = false; 484 | }; 485 | 486 | int main(int argc, char * argv[]) 487 | { 488 | ros::init(argc, argv, "tb3_0_FE_node"); 489 | FrontExpl FE; 490 | FE.main_loop(); 491 | ros::spin(); 492 | 493 | return 0; 494 | } -------------------------------------------------------------------------------- /src/tb3_1_FE.cpp: -------------------------------------------------------------------------------- 1 | /// \file 2 | /// \brief This node causes the robot to move autonomously via Frontier Exploration 3 | 4 | /// PUBLISHES: 5 | /// /edges_map_1 (nav_msgs/OccupancyGrid): Publishes the same map as SLAM, with visualization of the frontier edges 6 | /// SUBSCRIBES: 7 | /// /map (nav_msgs/OccupancyGrid): Reads the map created by SLAM to determine frontiers 8 | /// SERVICES: 9 | /// /tb3_1_start (bool): Starts the robot's frontier exploraiton algorithm 10 | 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 | #include 26 | 27 | 28 | class FrontExpl 29 | { 30 | public: 31 | FrontExpl() 32 | { 33 | FE1_map_pub = nh.advertise("edges_map_1", 1); 34 | map_1_sub = nh.subscribe("map", 10, &FrontExpl::mapCallback, this); 35 | // tf2_ros::TransformListener tfListener(tfBuffer); 36 | // MoveBaseClient ac("tb3_1/move_base", true); 37 | start1_srv = nh.advertiseService("tb3_1_start", &FrontExpl::startCallback, this); 38 | std::cout << "Initialized tb3_1 publishers, subcribers and service" << std::endl; 39 | } 40 | 41 | /// \brief Starts frontier exploration 42 | /// \param req - service request 43 | /// \param res - service response 44 | /// \returns true when done 45 | bool startCallback(multi_robot_exploration::tb3_1_start::Request &req, multi_robot_exploration::tb3_1_start::Response &res) 46 | { 47 | std::cout << "Got to start tb3_1 service" << std::endl; 48 | start1_flag = true; 49 | return true; 50 | } 51 | 52 | /// \brief Reads the map data published from slam_toolbox 53 | /// \param msg - map message 54 | /// \returns nothing 55 | void mapCallback(const nav_msgs::OccupancyGrid & msg) 56 | { 57 | FE1_map = msg; 58 | FE1_map.header.frame_id = "edges_map_1"; 59 | std::cout << "Got to map0 callback" << std::endl; 60 | } 61 | 62 | /// \brief Stores a vector of index values of the 8 cell neighborhood relative to the input cell 63 | /// \param cell - map cell 64 | /// \returns nothing 65 | void neighborhood(int cell) 66 | { 67 | // Clear any previous values in the vectors 68 | neighbor1_index.clear(); 69 | neighbor1_value.clear(); 70 | 71 | // Store neighboring values and indexes 72 | neighbor1_index.push_back(cell - map_width - 1); 73 | neighbor1_index.push_back(cell - map_width); 74 | neighbor1_index.push_back(cell - map_width + 1); 75 | neighbor1_index.push_back(cell-1); 76 | neighbor1_index.push_back(cell+1); 77 | neighbor1_index.push_back(cell + map_width - 1); 78 | neighbor1_index.push_back(cell + map_width); 79 | neighbor1_index.push_back(cell + map_width + 1); 80 | 81 | sort( neighbor1_index.begin(), neighbor1_index.end() ); 82 | neighbor1_index.erase( unique( neighbor1_index.begin(), neighbor1_index.end() ), neighbor1_index.end() ); 83 | } 84 | 85 | /// \brief Stores a vector of frontier edges 86 | /// \returns nothing 87 | void find_all_edges() 88 | { 89 | std::cout << "Finding all the tb3_1 edges" << std::endl; 90 | 91 | // Starting one row up and on space in on the map so there are no indexing issues 92 | for (double x = map_width + 1; x < (map_width * map_height) - map_width - 1; x++) 93 | { 94 | // For all cells in the map, check if a cell is unknown 95 | if (FE1_map.data.at(x) == -1) 96 | { 97 | // If there is an unknown cell, then check neighboring cells to find a potential frontier edge (free cell) 98 | neighborhood(x); 99 | 100 | for(int i = 0; i < neighbor1_index.size(); i++) // For all neighboring cells 101 | { 102 | if (FE1_map.data.at(neighbor1_index.at(i)) == 0) 103 | { 104 | // If one of the neighboring cells is free, store it in the edges vector 105 | FE1_map.data.at(neighbor1_index.at(i)) = 10; // Visualize the frontier edge cells 106 | edge1_vec.push_back(neighbor1_index.at(i)); 107 | 108 | } 109 | } 110 | 111 | } 112 | } 113 | 114 | } 115 | 116 | /// \brief Compares two cells to see if the are identical or unique 117 | /// \param - curr_cell: The current cell being evaluated 118 | /// \param - next_cell: The next cell to be evaluated 119 | /// \returns true or false 120 | bool check_edges(int curr_cell, int next_cell) 121 | { 122 | if (curr_cell == next_cell) 123 | { 124 | return false; 125 | } 126 | else 127 | { 128 | return true; 129 | } 130 | } 131 | 132 | /// \brief Given the frontier edges, group the neighboring edges into regions 133 | /// \returns nothing 134 | void find_regions() 135 | { 136 | std::cout << "Finding regions for tb3_1" << std::endl; 137 | 138 | for (int q = 0; q < edge1_vec.size() - 1; q++) 139 | { 140 | // For each frontier edge, check that the next value is unique and not a repeat 141 | unique_flag = check_edges(edge1_vec.at(q), edge1_vec.at(q+1)); 142 | 143 | if (unique_flag == true) 144 | { 145 | // If we have an original frontier edge, check the neighboring cells 146 | neighborhood(edge1_vec.at(q)); 147 | 148 | for(int i = 0; i < neighbor1_index.size(); i++) 149 | { 150 | // For all the cells nighrboring the frontier edge, check to see if there is another frontier edge 151 | if (neighbor1_index.at(i) == edge1_vec.at(q+1)) 152 | { 153 | // If a frontier edge is in the neightborhood of another frontier edge, add it to a region 154 | edge_index = edge1_vec.at(q); 155 | temp_group0.push_back(edge1_vec.at(q));; 156 | 157 | sort( temp_group0.begin(), temp_group0.end() ); 158 | temp_group0 .erase( unique( temp_group0.begin(), temp_group0.end() ), temp_group0.end() ); 159 | 160 | // Increase the counter to keep track of the region's size 161 | group1_c++; 162 | } 163 | } 164 | 165 | if (group1_c == prev_group1_c) // If we didnt any any edeges to our region, region is complete 166 | { 167 | if (group1_c < 5) // frontier region too small 168 | { 169 | // If the forntier region is smaller than 5 cells, dont use it 170 | } 171 | 172 | else 173 | { 174 | // If the frontier region is larger than 5 cells, find the regions centroid 175 | centroid0 = (temp_group0.size()) / 2; 176 | centroid1_index = temp_group0.at(centroid0); 177 | centroids0.push_back(centroid1_index); 178 | } 179 | 180 | // Reset the region vector and size counter 181 | group1_c = 0; 182 | temp_group0.clear(); 183 | } 184 | 185 | else 186 | { 187 | // If we found a frontier edge, increase the group size 188 | prev_group1_c = group1_c; 189 | } 190 | } 191 | 192 | else 193 | { 194 | // If we got a duplicate cell, do nothing 195 | } 196 | 197 | } 198 | 199 | } 200 | 201 | /// \brief Finds the transform between the map frame and the robot's base_footprint frame 202 | /// \returns nothing 203 | void find_transform() 204 | { 205 | // Find current location and move to the nearest centroid 206 | // Get robot pose 207 | transformS = tfBuffer.lookupTransform(map1_frame, body1_frame, ros::Time(0), ros::Duration(3.0)); 208 | 209 | transformS.header.stamp = ros::Time(); 210 | transformS.header.frame_id = body1_frame; 211 | transformS.child_frame_id = map1_frame; 212 | 213 | robot1_pose_.position.x = transformS.transform.translation.x; 214 | robot1_pose_.position.y = transformS.transform.translation.y; 215 | robot1_pose_.position.z = 0.0; 216 | robot1_pose_.orientation = transformS.transform.rotation; 217 | // robot1_pose_.orientation.y = transformS.transform.rotation.y; 218 | // robot1_pose_.orientation.z = transformS.transform.rotation.z; 219 | // robot1_pose_.orientation.w = transformS.transform.rotation.w; 220 | 221 | std::cout << "Robot pose is " << robot1_pose_.position.x << " , " << robot1_pose_.position.y << std::endl; 222 | } 223 | 224 | /// \brief Given a centroid cell, convert the centroid to x-y coordinates in the map frame and determine its distance from the robot 225 | /// \returns nothing 226 | void centroid_index_to_point() 227 | { 228 | for (int t = 0; t < centroids0.size(); t++) 229 | { 230 | // For all the centorid cells, find the x and y coordinates in the map frame 231 | point.x = (centroids0.at(t) % FE1_map.info.width)*FE1_map.info.resolution + FE1_map.info.origin.position.x; 232 | point.y = floor(centroids0.at(t) / FE1_map.info.width)*FE1_map.info.resolution + FE1_map.info.origin.position.y; 233 | 234 | for (int w = 0; w < prev_cent_1x.size(); w++) 235 | { 236 | // Compare the previous centroids to the current calculated centroid 237 | if ( (fabs( prev_cent_1x.at(w) - point.x) < 0.01) && (fabs( prev_cent_1y.at(w) - point.y) < 0.01) ) 238 | { 239 | // If the current centroid is too close to a previous centroid, skip 240 | std::cout << "Already went to this centroid " << prev_cent_1x.at(w) << " , " << prev_cent_1y.at(w) << std::endl; 241 | goto bad_centroid; 242 | } 243 | } 244 | 245 | if((point.x < FE1_map.info.origin.position.x + 0.05) && (point.y < FE1_map.info.origin.position.y + 0.05)) 246 | { 247 | // If the centroid is too close to the map orgin, skip 248 | goto bad_centroid; 249 | } 250 | 251 | else 252 | { 253 | // If the centroid is valid, add its x and y values to their respective vectors 254 | centroid1_Xpts.push_back(point.x); 255 | centroid1_Ypts.push_back(point.y); 256 | 257 | // Determine the distance between the current centroid and the robot's position 258 | double delta_x = point.x - robot1_pose_.position.x; 259 | double delta_y = point.y - robot1_pose_.position.y; 260 | double sum = (pow(delta_x ,2)) + (pow(delta_y ,2)); 261 | dist0 = pow( sum , 0.5 ); 262 | 263 | // Store the distance value in a vector 264 | dist1_arr.push_back(dist0); 265 | } 266 | 267 | // Skip to the end of the for loop if there was an invalid centroid 268 | bad_centroid: 269 | std::cout << "Ignore bad centroid" << std::endl; 270 | } 271 | } 272 | 273 | /// \brief Given all the centroid distance values, find the closest centroid to move to 274 | /// \returns nothing 275 | void find_closest_centroid() 276 | { 277 | // Set the first smallest distance to be large 278 | smallest = 9999999.0; 279 | for(int u = 0; u < dist1_arr.size(); u++) 280 | { 281 | // For each centroid distance, determine if the current centroid is closer than the previous closest centroid 282 | 283 | if (dist1_arr.at(u) < 0.1) 284 | { 285 | // If the centroid distance is less than 0.1m, its too close. Ignore it 286 | } 287 | else if (dist1_arr.at(u) < smallest) 288 | { 289 | // If the current distance element is smaller than the previous closest centroid 290 | // replace the smallest distance variable with the current distance 291 | smallest = dist1_arr.at(u); 292 | 293 | // Update the centroid that the robot will move to 294 | move_to_pt = u; 295 | } 296 | 297 | else 298 | { 299 | // If the current distance value is larger than 0.1 and the smallest_dist value, then ignore it 300 | } 301 | } 302 | } 303 | 304 | /// \brief Given the frontier edges, convert the cell to x-y coordinates in the map frame and determine its distance from the robot 305 | /// \returns nothing 306 | void edge_index_to_point() 307 | { 308 | for (int t = 0; t < edge1_vec.size(); t++) 309 | { 310 | // For all the frontier edge cells, find the x and y coordinates in the map frame 311 | point.x = (edge1_vec.at(t) % FE1_map.info.width)*FE1_map.info.resolution + FE1_map.info.origin.position.x; 312 | point.y = floor(edge1_vec.at(t) / FE1_map.info.width)*FE1_map.info.resolution + FE1_map.info.origin.position.y; 313 | 314 | // Add the cells x and y values to their respective vectors 315 | centroid1_Xpts.push_back(point.x); 316 | centroid1_Ypts.push_back(point.y); 317 | 318 | // Determine the distance between the current frontier edge and the robot's position 319 | double delta_x = point.x - robot1_pose_.position.x; 320 | double delta_y = point.y - robot1_pose_.position.y; 321 | double sum = (pow(delta_x ,2)) + (pow(delta_y ,2)); 322 | dist0 = pow( sum , 0.5 ); 323 | 324 | // Store the distance value in a vector 325 | dist1_arr.push_back(dist0); 326 | } 327 | } 328 | 329 | /// \brief Calls all other functions to find frontier edges, regions and a goal to move to. Then uses the action server to move to that goal 330 | /// \returns nothing 331 | void main_loop() 332 | { 333 | std::cout << "Entered the main loop for 0" << std::endl; 334 | 335 | typedef actionlib::SimpleActionClient MoveBaseClient; 336 | MoveBaseClient ac("tb3_1/move_base", true); 337 | 338 | // Wait for the action server to come up 339 | while(!ac.waitForServer(ros::Duration(5.0))) 340 | { 341 | ROS_INFO("Waiting for the move_base action server to come up"); 342 | } 343 | ROS_INFO("Connected to move base server"); 344 | 345 | tf2_ros::TransformListener tfListener(tfBuffer); 346 | ros::Rate loop_rate(40); 347 | 348 | while (ros::ok()) 349 | { 350 | std::cout << "While ros okay for tb3_1" << std::endl; 351 | ros::spinOnce(); 352 | 353 | // If there is no map data and / or start service isnt called, do nothing and instead start the loop over again 354 | if (FE1_map.data.size()!=0 && start1_flag == true) 355 | { 356 | map_width = FE1_map.info.width; 357 | map_height = FE1_map.info.height; 358 | 359 | // Find the frontier edges 360 | find_all_edges(); 361 | 362 | // If there are no frontier edges, skip to the end of the main_loop and try again 363 | if (edge1_vec.size() == 0) 364 | { 365 | goto skip; 366 | } 367 | 368 | sort( edge1_vec.begin(), edge1_vec.end() ); 369 | edge1_vec.erase( unique( edge1_vec.begin(), edge1_vec.end() ), edge1_vec.end() ); 370 | 371 | FE1_map_pub.publish(FE1_map); 372 | // Given the forntier edges, find the frontier regions and their centroids 373 | find_regions(); 374 | 375 | // Find the transfrom between the map frame the base_footprint frame 376 | // in order to determine the robot's position 377 | find_transform(); 378 | 379 | // Given the centroid vector, convert the controids from map cells to x-y coordinates in the map frame 380 | // so a goal can be sent to move_base 381 | centroid_index_to_point(); 382 | 383 | // If there are no values in the centroid x or y vectors, find the closest frontier edge to move to 384 | if ( ( centroid1_Xpts.size() == 0 ) || ( centroid1_Ypts.size() == 0) ) 385 | { 386 | centroid1_Xpts.clear(); 387 | centroid1_Ypts.clear(); 388 | dist1_arr.clear(); 389 | std::cout << "Couldnt find a centroid, move to closest edge instead" << std::endl; 390 | 391 | // Given the edge vector, convert the edges from map cells to x-y coordinates in the map frame 392 | // so a goal can be sent to move_base 393 | edge_index_to_point(); 394 | } 395 | 396 | // Of all the centroids, determine which centroid is the closest 397 | // Choose the closest centroid to move to 398 | find_closest_centroid(); 399 | 400 | std::cout << "Moving to centroid " << centroid1_Xpts.at(move_to_pt) << " , " << centroid1_Ypts.at(move_to_pt) << std::endl; 401 | std::cout << "At a distance of " << smallest << std::endl; 402 | 403 | // Add the current centroid to the vector of previously visited centroids 404 | prev_cent_1x.push_back(centroid1_Xpts.at(move_to_pt)); 405 | prev_cent_1y.push_back(centroid1_Ypts.at(move_to_pt)); 406 | 407 | // Determine the move_base goal 408 | goal.target_pose.header.frame_id = "tb3_1/map"; // Needs to be AN ACTUAL FRAME 409 | // goal.target_pose.header.frame_id = "map"; // Needs to be AN ACTUAL FRAME 410 | goal.target_pose.header.stamp = ros::Time(); 411 | goal.target_pose.pose.position.x = centroid1_Xpts.at(move_to_pt); 412 | goal.target_pose.pose.position.y = centroid1_Ypts.at(move_to_pt); 413 | goal.target_pose.pose.orientation.w = 1.0; 414 | 415 | ROS_INFO("Sending goal"); 416 | ac.sendGoal(goal); 417 | 418 | // Wait for the action to return 419 | ac.waitForResult(ros::Duration(60)); 420 | 421 | if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 422 | { 423 | ROS_INFO("You have reached the goal!"); 424 | } 425 | else 426 | { 427 | ROS_INFO("The base failed for some reason"); 428 | } 429 | 430 | std::cout << "" << std::endl; 431 | } 432 | 433 | // If the size of the map data is 0, then run through the loop agai 434 | else 435 | { 436 | ROS_INFO("Waiting for first map"); 437 | } 438 | 439 | // Skip to the end of the loop if there are no fontier regions 440 | skip: 441 | std::cout << "Starting loop over" << std::endl; 442 | sleep(1.0); 443 | 444 | std::cout << "Resetting vairbales and clearing all vectors" << std::endl; 445 | centroid0 = 0; 446 | centroid1_index = 0; 447 | dist1_arr.clear(); 448 | edge1_vec.clear(); 449 | neighbor1_index.clear(); 450 | neighbor1_value.clear(); 451 | centroids0.clear(); 452 | centroid1_Xpts.clear(); 453 | centroid1_Ypts.clear(); 454 | 455 | ros::spinOnce(); 456 | 457 | std::cout << "Spinning like a ballerina" << std::endl; 458 | 459 | loop_rate.sleep(); 460 | 461 | std::cout << "Asleep and done with main_loop" << std::endl; 462 | } 463 | } 464 | 465 | private: 466 | ros::NodeHandle nh; 467 | ros::Publisher FE1_map_pub; 468 | ros::Subscriber map_1_sub; 469 | ros::ServiceServer start1_srv; 470 | tf2_ros::Buffer tfBuffer; 471 | nav_msgs::OccupancyGrid FE1_map; 472 | geometry_msgs::Pose robot1_pose_; 473 | geometry_msgs::Point point; 474 | geometry_msgs::TransformStamped transformS; 475 | move_base_msgs::MoveBaseGoal goal; 476 | typedef actionlib::SimpleActionClient MoveBaseClient; 477 | std::string map1_frame = "tb3_1/map"; 478 | std::string body1_frame = "tb3_1/base_footprint"; 479 | std::vector edge1_vec, neighbor1_index, neighbor1_value; 480 | std::vector centroids0, temp_group0; 481 | std::vector centroid1_Xpts, centroid1_Ypts, dist1_arr, prev_cent_1x, prev_cent_1y; 482 | int group1_c=0, prev_group1_c=0, centroid0=0, centroid1_index=0, move_to_pt=0, map_width=0, map_height=0, mark_edge=0, edge_index=0; 483 | double smallest = 9999999.0, dist0= 0.0; 484 | bool unique_flag = true; 485 | bool start1_flag = false; 486 | }; 487 | 488 | int main(int argc, char * argv[]) 489 | { 490 | ros::init(argc, argv, "tb3_1_FE_node"); 491 | FrontExpl FE; 492 | FE.main_loop(); 493 | ros::spin(); 494 | 495 | return 0; 496 | } -------------------------------------------------------------------------------- /srv/tb3_0_start.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | bool got_msg -------------------------------------------------------------------------------- /srv/tb3_1_start.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | bool got_msg -------------------------------------------------------------------------------- /urdf/tb3_0.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | Gazebo/DarkGrey 8 | 9 | 10 | 11 | 0.1 12 | 0.1 13 | 500000.0 14 | 10.0 15 | 0.001 16 | 0.1 17 | 1 0 0 18 | Gazebo/FlatBlack 19 | 20 | 21 | 22 | 0.1 23 | 0.1 24 | 500000.0 25 | 10.0 26 | 0.001 27 | 0.1 28 | 1 0 0 29 | Gazebo/FlatBlack 30 | 31 | 32 | 33 | 0.1 34 | 0.1 35 | 1000000.0 36 | 100.0 37 | 0.001 38 | 1.0 39 | Gazebo/FlatBlack 40 | 41 | 42 | 43 | 44 | true 45 | $(arg imu_visual) 46 | 47 | Gazebo/FlatBlack 48 | 49 | 50 | 51 | 52 | cmd_vel 53 | odom 54 | odom 55 | world 56 | true 57 | base_footprint 58 | false 59 | true 60 | true 61 | false 62 | 30 63 | tb3_0/wheel_left_joint 64 | tb3_0/wheel_right_joint 65 | 0.160 66 | 0.066 67 | 1 68 | 10 69 | na 70 | 71 | 72 | 73 | 74 | 75 | true 76 | tb3_0/imu_link 77 | tb3_0/imu_link 78 | imu 79 | imu_service 80 | 0.0 81 | 200 82 | 83 | 84 | gaussian 85 | 86 | 0.0 87 | 2e-4 88 | 0.0000075 89 | 0.0000008 90 | 91 | 92 | 0.0 93 | 1.7e-2 94 | 0.1 95 | 0.001 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | Gazebo/FlatBlack 104 | 105 | 0 0 0 0 0 0 106 | $(arg laser_visual) 107 | 5 108 | 109 | 110 | 111 | 360 112 | 1 113 | 0.0 114 | 6.28319 115 | 116 | 117 | 118 | 0.120 119 | 3.5 120 | 0.015 121 | 122 | 123 | gaussian 124 | 0.0 125 | 0.01 126 | 127 | 128 | 129 | scan 130 | base_scan 131 | 132 | 133 | 134 | 135 | -------------------------------------------------------------------------------- /urdf/tb3_0.urdf.xacro: -------------------------------------------------------------------------------- 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 | 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 | 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 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 162 | 163 | 164 | 165 | -------------------------------------------------------------------------------- /urdf/tb3_1.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | Gazebo/DarkGrey 8 | 9 | 10 | 11 | 0.1 12 | 0.1 13 | 500000.0 14 | 10.0 15 | 0.001 16 | 0.1 17 | 1 0 0 18 | Gazebo/FlatBlack 19 | 20 | 21 | 22 | 0.1 23 | 0.1 24 | 500000.0 25 | 10.0 26 | 0.001 27 | 0.1 28 | 1 0 0 29 | Gazebo/FlatBlack 30 | 31 | 32 | 33 | 0.1 34 | 0.1 35 | 1000000.0 36 | 100.0 37 | 0.001 38 | 1.0 39 | Gazebo/FlatBlack 40 | 41 | 42 | 43 | 44 | true 45 | $(arg imu_visual) 46 | 47 | Gazebo/FlatBlack 48 | 49 | 50 | 51 | 52 | cmd_vel 53 | odom 54 | odom 55 | world 56 | true 57 | base_footprint 58 | false 59 | true 60 | true 61 | false 62 | 30 63 | tb3_1/wheel_left_joint 64 | tb3_1/wheel_right_joint 65 | 0.160 66 | 0.066 67 | 1 68 | 10 69 | na 70 | 71 | 72 | 73 | 74 | 75 | true 76 | tb3_1/imu_link 77 | tb3_1/imu_link 78 | imu 79 | imu_service 80 | 0.0 81 | 200 82 | 83 | 84 | gaussian 85 | 86 | 0.0 87 | 2e-4 88 | 0.0000075 89 | 0.0000008 90 | 91 | 92 | 0.0 93 | 1.7e-2 94 | 0.1 95 | 0.001 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | Gazebo/FlatBlack 104 | 105 | 0 0 0 0 0 0 106 | $(arg laser_visual) 107 | 5 108 | 109 | 110 | 111 | 360 112 | 1 113 | 0.0 114 | 6.28319 115 | 116 | 117 | 118 | 0.120 119 | 3.5 120 | 0.015 121 | 122 | 123 | gaussian 124 | 0.0 125 | 0.01 126 | 127 | 128 | 129 | scan 130 | base_scan 131 | 132 | 133 | 134 | 135 | 136 | -------------------------------------------------------------------------------- /urdf/tb3_1.urdf.xacro: -------------------------------------------------------------------------------- 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 | 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 | 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 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 162 | 163 | 164 | 165 | 166 | -------------------------------------------------------------------------------- /worlds/bookstore.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -5.73 3.76 8.10 0 1.1316 -0.4398 8 | 9 | 10 | 0 0 -9.8 11 | 6e-06 2.3e-05 -4.2e-05 12 | 13 | 14 | 0.001 15 | 1 16 | 1000 17 | 18 | 19 | 0.4 0.4 0.4 1 20 | 0.7 0.7 0.7 1 21 | 1 22 | false 23 | false 24 | 25 | 26 | EARTH_WGS84 27 | 0 28 | 0 29 | 0 30 | 0 31 | 32 | 33 | 34 | 35 | 36 | model://aws_robomaker_retail_AirConditionerC_01 37 | 38 | 7.458202 1.422453 3.290944 0 -0 -1.564217 39 | 40 | 41 | 42 | 43 | 44 | model://aws_robomaker_retail_Bicycle_01 45 | 46 | 5.681248 -2.242451 0.931246 0.000002 -0.000024 0 47 | 48 | 49 | 50 | 51 | 52 | 53 | model://aws_robomaker_retail_BookE_01 54 | 55 | 5.476534 -1.932532 0.948933 0.000004 0.000001 -2.543521 56 | 57 | 58 | 59 | 60 | model://aws_robomaker_retail_BookE_01 61 | 62 | -5.414000 3.330672 0.934015 -0.000001 -0.000001 -1.541094 63 | 64 | 65 | 66 | 67 | model://aws_robomaker_retail_BookE_01 68 | 69 | 5.552553 -1.214211 0.948933 0.000004 -0.000001 -2.654312 70 | 71 | 72 | 73 | 74 | model://aws_robomaker_retail_BookE_01 75 | 76 | 5.677231 -4.649203 0.946394 0.000100 0.000006 -2.355752 77 | 78 | 79 | 80 | 81 | model://aws_robomaker_retail_BookE_01 82 | 83 | 5.538150 -1.610317 0.948933 0 0 -2.526901 84 | 85 | 86 | 87 | 88 | model://aws_robomaker_retail_BookE_01 89 | 90 | -1.889425 3.346395 1.054941 0 0 1.568157 91 | 92 | 93 | 94 | 95 | model://aws_robomaker_retail_BookE_01 96 | 97 | -2.123726 3.346395 1.054941 0 0 1.568157 98 | 99 | 100 | 101 | 102 | 103 | model://aws_robomaker_retail_BookE_01 104 | 105 | -5.315121 1.771952 0.955242 3.141243 -0.072268 -2.526906 106 | 107 | 108 | 109 | 110 | model://aws_robomaker_retail_BookE_01 111 | 112 | 1.170199 1.196251 343582.718718 0.002241 -0.001100 -0.623328 113 | 114 | 115 | 116 | 117 | model://aws_robomaker_retail_BookE_01 118 | 119 | -5.503415 1.012063 0.941463 0 0 -1.568440 120 | 121 | 122 | 123 | 124 | model://aws_robomaker_retail_BookE_01 125 | 126 | 5.749231 -2.492018 0.948933 0.000002 0.000001 2.223394 127 | 128 | 129 | 130 | 131 | model://aws_robomaker_retail_BookE_01 132 | 133 | -5.333584 0.186186 0.949316 -0.000001 -0.004549 -2.798485 134 | 135 | 136 | 137 | 138 | model://aws_robomaker_retail_BookE_01 139 | 140 | -5.403906 2.650881 0.931119 0.021204 -0.000001 -2.355751 141 | 142 | 143 | 144 | 145 | model://aws_robomaker_retail_BookE_01 146 | 147 | -1.889425 2.993687 1.054941 0 0 1.568157 148 | 149 | 150 | 151 | 152 | model://aws_robomaker_retail_BookE_01 153 | 154 | -2.123726 2.993687 1.054941 0 0 1.568157 155 | 156 | 157 | 158 | 159 | model://aws_robomaker_retail_BookE_01 160 | 161 | -2.357369 2.993687 1.054941 0 0 1.568157 162 | 163 | 164 | 165 | 166 | model://aws_robomaker_retail_BookE_01 167 | 168 | -1.889425 3.172920 1.054941 0 0 1.568157 169 | 170 | 171 | 172 | 173 | model://aws_robomaker_retail_BookE_01 174 | 175 | -2.123726 3.172920 1.054941 0 0 1.568157 176 | 177 | 178 | 179 | 180 | 181 | model://aws_robomaker_retail_BookE_01 182 | 183 | 184 | -2.357369 3.172920 1.054941 0 0 1.568157 185 | 186 | 187 | 188 | 189 | model://aws_robomaker_retail_BookE_01 190 | 191 | -0.694312 -3.530823 1.053825 0.000004 -0.000004 -1.573382 192 | 193 | 194 | 195 | 196 | model://aws_robomaker_retail_BookE_01 197 | 198 | -0.694312 -3.760511 1.053825 0.000004 -0.000004 -1.573382 199 | 200 | 201 | 202 | 203 | model://aws_robomaker_retail_BookE_01 204 | 205 | -0.694312 -3.980472 1.053825 0.000004 -0.000004 -1.573382 206 | 207 | 208 | 209 | 210 | model://aws_robomaker_retail_BookE_01 211 | 212 | -0.694312 -4.221891 1.053825 0.000004 -0.000004 -1.573382 213 | 214 | 215 | 216 | 217 | model://aws_robomaker_retail_BookE_01 218 | 219 | -0.694312 -4.461891 1.053825 0.000004 -0.000004 -1.573382 220 | 221 | 222 | 223 | 224 | model://aws_robomaker_retail_BookE_01 225 | 226 | -0.694312 -4.701891 1.053825 0.000004 -0.000004 -1.573382 227 | 228 | 229 | 230 | 231 | 232 | model://aws_robomaker_retail_BookE_01 233 | 234 | -0.694312 -4.941891 1.053825 0.000004 -0.000004 -1.573382 235 | 236 | 237 | 238 | 239 | model://aws_robomaker_retail_BookE_01 240 | 241 | -0.694312 -5.221891 1.053825 0.000004 -0.000004 -1.573382 242 | 243 | 244 | 245 | 246 | model://aws_robomaker_retail_BookE_01 247 | 248 | -0.694312 -3.291891 1.053825 0.000004 -0.000004 -1.573382 249 | 250 | 251 | 252 | 253 | model://aws_robomaker_retail_BookE_01 254 | 255 | -0.694312 -3.021891 1.053825 0.000004 -0.000004 -1.573382 256 | 257 | 258 | 259 | 260 | 261 | model://aws_robomaker_retail_BookE_01 262 | 263 | -2.357369 3.346395 1.054941 0 0 1.568157 264 | 265 | 266 | 267 | 268 | 269 | model://aws_robomaker_retail_BookF_01 270 | 271 | 2.931779 -1.303250 0.770041 0.003448 -0.000011 0.003114 272 | 273 | 274 | 275 | 276 | model://aws_robomaker_retail_BookD_01 277 | 278 | 5.720048 -5.365876 0.022179 0.003452 -0.000001 0.000652 279 | 280 | 281 | 282 | 283 | model://aws_robomaker_retail_BookF_01 284 | 285 | 2.902979 -4.760748 0.75826 0.003450 0.000006 -0.002250 286 | 287 | 288 | 289 | 290 | model://aws_robomaker_retail_BookD_01 291 | 292 | -4.509612 -1.312536 0.804482 0 0 -0.000004 293 | 294 | 295 | 296 | 297 | model://aws_robomaker_retail_BookF_01 298 | 299 | -4.604931 -4.772141 0.804483 0.000004 0 0 300 | 301 | 302 | 303 | 304 | model://aws_robomaker_retail_BookD_01 305 | 306 | -6.235609 -3.785181 1.038157 0.000008 0 1.580378 307 | 308 | 309 | 310 | 311 | model://aws_robomaker_retail_BookF_01 312 | 313 | 1.261644 0.856469 1.038157 0.000008 0 1.580378 314 | 315 | 316 | 317 | 318 | model://aws_robomaker_retail_BookD_01 319 | 320 | 2.358626 0.854853 1.038153 0.000006 0 0.001712 321 | 322 | 323 | 324 | 325 | model://aws_robomaker_retail_BookF_01 326 | 327 | 3.430611 0.856469 1.038157 0.000008 0 1.580378 328 | 329 | 330 | 331 | 332 | model://aws_robomaker_retail_BookshelfA_01 333 | 334 | 1.405153 -1.449740 0.03755 0.003490 -0.000003 0.000696 335 | 336 | 337 | 338 | 339 | model://aws_robomaker_retail_BookshelfA_01 340 | 341 | 1.405153 -4.761649 0.003411 0 0 0 342 | 343 | 344 | 345 | 346 | model://aws_robomaker_retail_BookshelfA_01 347 | 348 | -2.554493 -1.487456 0.006337 0.001313 -0.001987 0.031824 349 | 350 | 351 | 352 | 353 | model://aws_robomaker_retail_BookshelfA_01 354 | 355 | -2.608578 -4.761649 0.003411 0 0 0 356 | 357 | 358 | 359 | 360 | model://aws_robomaker_retail_BookshelfB_01 361 | 362 | -6.189675 -6.785646 -0.008942 0.003533 0.000121 -0.032073 363 | 364 | 365 | 366 | 367 | model://aws_robomaker_retail_BookshelfB_01 368 | 369 | -2.209411 -6.971459 0.017074 -0.000128 -0.000252 0.000364 370 | 371 | 372 | 373 | 374 | model://aws_robomaker_retail_BookshelfB_01 375 | 376 | 0.522246 -6.971500 0.017947 -0.003292 0.000000 0.000000 377 | 378 | 379 | 380 | 381 | model://aws_robomaker_retail_BookshelfB_01 382 | 383 | 3.293386 -6.963665 0.017383 0.000008 -0.000002 0.000000 384 | 385 | 386 | 387 | 388 | model://aws_robomaker_retail_BookshelfB_01 389 | 390 | 6.080380 -6.987659 0.017409 0.000174 0.000004 0.000000 391 | 392 | 393 | 394 | 395 | model://aws_robomaker_retail_BookshelfB_01 396 | 397 | 7.478331 -5.298296 0.017443 -0.001974 -0.000211 1.554057 398 | 399 | 400 | 401 | 402 | model://aws_robomaker_retail_BookshelfB_01 403 | 404 | 7.478331 -2.510651 0.017402 0.000010 -0.000013 1.562068 405 | 406 | 407 | 408 | 409 | model://aws_robomaker_retail_BookshelfB_01 410 | 411 | 7.478331 0.402539 0.017181 0.000003 -0.000156 1.570798 412 | 413 | 414 | 415 | 416 | model://aws_robomaker_retail_BookshelfC_01 417 | 418 | 2.911133 -1.310229 0.008469 0.003452 -0.000003 0.000620 419 | 420 | 421 | 422 | 423 | model://aws_robomaker_retail_BookshelfC_01 424 | 425 | 2.915449 -4.761712 -0.003455 0.003452 0 0.000014 426 | 427 | 428 | 429 | 430 | model://aws_robomaker_retail_BookshelfC_01 431 | 432 | -4.517110 -1.310229 0.045908 0 0 0 433 | 434 | 435 | 436 | 437 | model://aws_robomaker_retail_BookshelfC_01 438 | 439 | -4.614360 -4.761649 0.045908 0 0 0 440 | 441 | 442 | 443 | 444 | 452 | 453 | 454 | 462 | 463 | 464 | 465 | model://aws_robomaker_retail_ChairB_01 466 | 467 | 6.6439554 -0.210457 0.0220513 0 0.006174 0.000088 468 | 469 | 470 | 471 | 472 | model://aws_robomaker_retail_ChairB_01 473 | 474 | 6.6439554 -1.621987 0.0220513 0 0.006174 0.000088 475 | 476 | 477 | 478 | 479 | model://aws_robomaker_retail_ChairB_01 480 | 481 | 6.6439554 -3.246633 0.0220513 0 0.006174 0.000088 482 | 483 | 484 | 485 | 486 | model://aws_robomaker_retail_ChairB_01 487 | 488 | 6.6439554 -4.681600 0.0220513 0 0.006174 0.000088 489 | 490 | 491 | 492 | 493 | 494 | model://aws_robomaker_retail_ChairB_01 495 | 496 | 4.653303 -0.210457 0.0220513 0 0.006174 0.000088 497 | 498 | 499 | 500 | 501 | model://aws_robomaker_retail_ChairB_01 502 | 503 | 4.653303 -1.621987 0.0220513 0 0.006174 0.000088 504 | 505 | 506 | 507 | 508 | model://aws_robomaker_retail_ChairB_01 509 | 510 | 4.653303 -3.246633 0.0220513 0 0.006174 0.000088 511 | 512 | 513 | 514 | 515 | model://aws_robomaker_retail_ChairB_01 516 | 517 | 4.653303 -4.681600 0.0220513 0 0.006174 0.000088 518 | 519 | 520 | 521 | 522 | model://aws_robomaker_retail_ChairB_01 523 | 524 | -4.602947 5.021919 0.030259 -0.001073 -0.001935 0.001439 525 | 526 | 527 | 528 | 529 | model://aws_robomaker_retail_ChairB_01 530 | 531 | -6.109693 5.026245 0.027274 -0.001422 -0.002424 0.000176 532 | 533 | 534 | 535 | 536 | model://aws_robomaker_retail_ChairB_01 537 | 538 | -4.616166 3.276753 0.0321 -0.001068 -0.001938 0.000117 539 | 540 | 541 | 542 | 543 | model://aws_robomaker_retail_ChairB_01 544 | 545 | -6.090762 3.293415 0.029216 -0.001072 -0.001936 0.000119 546 | 547 | 548 | 549 | 550 | model://aws_robomaker_retail_ChairB_01 551 | 552 | -4.589604 1.815766 0.032459 0.001362 -0.001944 0.000145 553 | 554 | 555 | 556 | 557 | model://aws_robomaker_retail_ChairB_01 558 | 559 | -6.130103 1.798599 0.02944 0.001362 -0.001946 0.000105 560 | 561 | 562 | 563 | 564 | model://aws_robomaker_retail_ChairB_01 565 | 566 | -4.607505 0.203637 0.030218 0.001362 -0.001944 0.000163 567 | 568 | 569 | 570 | 571 | model://aws_robomaker_retail_ChairB_01 572 | 573 | -6.121492 0.195027 0.027272 0.001357 -0.001950 0.003291 574 | 575 | 576 | 577 | 578 | model://aws_robomaker_retail_ColumnB_01 579 | 580 | -4.076233 -6.860710 0.017511 0.001895 -0.000628 -0.003755 581 | 582 | 583 | 584 | 585 | model://aws_robomaker_retail_Computer_01 586 | 587 | 0.284724 4.951315 1.08517 -0.004006 -0.000097 -0.010660 588 | 589 | 590 | 591 | 592 | model://aws_robomaker_retail_SecurityCamera_01 593 | 594 | -1.308807 2.973710 1.037756 -0.004006 -0.000097 -0.010660 595 | 596 | 597 | 598 | 599 | model://aws_robomaker_retail_SecurityCamera_01 600 | 601 | -3.904507 3.590531 1.037756 0 0 3.050913 602 | 603 | 604 | 605 | 606 | model://aws_robomaker_retail_InfoDesk_01 607 | 608 | 1.010668 5.032576 0.011585 -0.006739 0.005110 -0.025146 609 | 610 | 611 | 612 | 613 | model://aws_robomaker_retail_DeskB_01 614 | 615 | 5.622942 -2.475419 -0.181282 0 -0 0 616 | 617 | 618 | 619 | 620 | model://aws_robomaker_retail_DeskB_01 621 | 622 | -5.400709 2.580536 -0.181282 0 -0 0 623 | 624 | 625 | 626 | 627 | model://aws_robomaker_retail_DeskA_01 628 | 629 | -2.340694 3.200118 0.014921 0.000124 -0.000010 0.000376 630 | 631 | 632 | 633 | 634 | model://aws_robomaker_retail_DeskA_01 635 | 636 | -2.369938 0.836099 0.00876 -0.000772 -0.001938 0.000017 637 | 638 | 639 | 640 | 641 | model://aws_robomaker_retail_DeskA_01 642 | 643 | -6.254654 -3.979197 -0.004603 0.001948 0.001361 -1.569567 644 | 645 | 646 | 647 | 648 | model://aws_robomaker_retail_DeskA_01 649 | 650 | -0.701688 -4.049294 0.014866 0.000000 -0.000000 -1.566097 651 | 652 | 653 | 654 | 655 | model://aws_robomaker_retail_DeskA_01 656 | 657 | 2.298197 0.861754 0.014883 -0.000002 0.003451 0.000024 658 | 659 | 660 | 661 | 662 | model://aws_robomaker_retail_DoorB_01 663 | 664 | -2.492457 7.219370 -0.035203 0 -0 0 665 | 666 | 667 | 668 | 669 | model://aws_robomaker_retail_RetailShopFloor_01 670 | 671 | -0.002687 0 -0.276618 -0 0 0 672 | 673 | 674 | 675 | 676 | model://aws_robomaker_retail_TabletB_01 677 | 678 | -0.887658 -2.526304 1.181088 0.534314 -0.007303 -3.125039 679 | 680 | 681 | 682 | 683 | model://aws_robomaker_retail_TabletB_01 684 | 685 | -0.490858 -2.526304 1.181088 0.534314 -0.007303 -3.125039 686 | 687 | 688 | 689 | 690 | model://aws_robomaker_retail_TabletB_01 691 | 692 | -0.598457 -5.570730 1.184718 0.550862 -0.007302 0.000000 693 | 694 | 695 | 696 | 697 | model://aws_robomaker_retail_TabletB_01 698 | 699 | -1.036580 -5.570730 1.184718 0.550862 -0.007302 0.000000 700 | 701 | 702 | 703 | 704 | model://aws_robomaker_retail_TabletB_01 705 | 706 | -0.378561 -4.954985 1.184718 0.518383 0.023342 1.564514 707 | 708 | 709 | 710 | 711 | model://aws_robomaker_retail_TabletB_01 712 | 713 | -0.378561 -4.227170 1.184718 0.518383 0.023342 1.564514 714 | 715 | 716 | 717 | 718 | model://aws_robomaker_retail_TabletB_01 719 | 720 | -0.378561 -3.280550 1.184718 0.518383 0.023342 1.564514 721 | 722 | 723 | 724 | 725 | model://aws_robomaker_retail_TabletB_01 726 | 727 | -1.003855 -4.964849 1.184718 0.512461 -0.058405 -1.604103 728 | 729 | 730 | 731 | 732 | model://aws_robomaker_retail_TabletB_01 733 | 734 | -1.003855 -4.224020 1.184718 0.512461 -0.058405 -1.604103 735 | 736 | 737 | 738 | 739 | model://aws_robomaker_retail_TabletB_01 740 | 741 | -1.003855 -3.275521 1.184718 0.512461 -0.058405 -1.604103 742 | 743 | 744 | 745 | 746 | model://aws_robomaker_retail_TabletB_01 747 | 748 | -5.915050 -3.031740 1.184718 0.518384 0.023341 1.564511 749 | 750 | 751 | 752 | 753 | model://aws_robomaker_retail_TabletB_01 754 | 755 | -5.915050 -4.612402 1.184718 0.518384 0.023341 1.564511 756 | 757 | 758 | 759 | 760 | model://aws_robomaker_retail_Tablet_01 761 | 762 | -1.846095 0.489109 1.180551 0.579329 0.041534 -0.007936 763 | 764 | 765 | 766 | 767 | model://aws_robomaker_retail_Tablet_01 768 | 769 | -2.650150 0.489109 1.180551 0.579329 0.041534 -0.007936 770 | 771 | 772 | 773 | 774 | model://aws_robomaker_retail_Tablet_01 775 | 776 | -1.069270 0.489109 1.180551 0.579329 0.041534 -0.007936 777 | 778 | 779 | 780 | 781 | model://aws_robomaker_retail_Tablet_01 782 | 783 | -3.639330 0.489109 1.180551 0.579329 0.041534 -0.007936 784 | 785 | 786 | 787 | 788 | model://aws_robomaker_retail_Tablet_01 789 | 790 | -1.072680 1.163816 1.177895 0.513308 -0.000001 3.110062 791 | 792 | 793 | 794 | 795 | model://aws_robomaker_retail_Tablet_01 796 | 797 | -1.876690 1.163816 1.177895 0.513308 -0.000001 3.110062 798 | 799 | 800 | 801 | 802 | model://aws_robomaker_retail_Tablet_01 803 | 804 | -2.649670 1.163816 1.177895 0.513308 -0.000001 3.110062 805 | 806 | 807 | 808 | 809 | model://aws_robomaker_retail_Tablet_01 810 | 811 | -3.636550 1.163816 1.177895 0.513308 -0.000001 3.110062 812 | 813 | 814 | 815 | model://aws_robomaker_retail_Holder_01 816 | 817 | -0.599181 -5.571190 1.037756 0 -0 0 818 | 819 | 820 | 821 | 822 | model://aws_robomaker_retail_Holder_01 823 | 824 | -0.491632 -2.528687 1.037756 0 -0 -3.130149 825 | 826 | 827 | 828 | 829 | model://aws_robomaker_retail_Holder_01 830 | 831 | -1.035840 -5.571191 1.037756 0 -0 0 832 | 833 | 834 | 835 | 836 | model://aws_robomaker_retail_Holder_01 837 | 838 | -0.886180 -2.529685 1.037756 0 -0 -3.130149 839 | 840 | 841 | 842 | 843 | model://aws_robomaker_retail_Holder_01 844 | 845 | -1.846620 0.495602 1.037756 0 -0 0 846 | 847 | 848 | 849 | 850 | model://aws_robomaker_retail_Holder_01 851 | 852 | -2.647810 0.495602 1.037756 0 -0 0 853 | 854 | 855 | 856 | 857 | model://aws_robomaker_retail_Holder_01 858 | 859 | -1.072420 0.495602 1.037756 0 -0 0 860 | 861 | 862 | 863 | 864 | model://aws_robomaker_retail_Holder_01 865 | 866 | -3.639583 0.495602 1.037756 0 -0 0 867 | 868 | 869 | 870 | 871 | model://aws_robomaker_retail_Holder_01 872 | 873 | -1.072932 1.160829 1.037756 0 -0 -3.130149 874 | 875 | 876 | 877 | 878 | model://aws_robomaker_retail_Holder_01 879 | 880 | -1.874890 1.160829 1.037756 0 -0 -3.130149 881 | 882 | 883 | 884 | 885 | model://aws_robomaker_retail_Holder_01 886 | 887 | -2.659160 1.160829 1.037756 0 -0 -3.130149 888 | 889 | 890 | 891 | 892 | model://aws_robomaker_retail_Holder_01 893 | 894 | -3.631300 1.160829 1.037756 0 -0 -3.130149 895 | 896 | 897 | 898 | 899 | model://aws_robomaker_retail_Holder_01 900 | 901 | -0.372529 -4.960362 1.037756 0 -0 1.550000 902 | 903 | 904 | 905 | 906 | model://aws_robomaker_retail_Holder_01 907 | 908 | -0.372529 -4.226824 1.037756 0 -0 1.550000 909 | 910 | 911 | 912 | 913 | model://aws_robomaker_retail_Holder_01 914 | 915 | -0.372529 -3.286318 1.037756 0 -0 1.550000 916 | 917 | 918 | 919 | 920 | model://aws_robomaker_retail_Holder_01 921 | 922 | -1.001700 -4.961520 1.037756 0 0 -1.588645 923 | 924 | 925 | 926 | 927 | model://aws_robomaker_retail_Holder_01 928 | 929 | -1.001700 -4.226100 1.037756 0 0 -1.588645 930 | 931 | 932 | 933 | 934 | model://aws_robomaker_retail_Holder_01 935 | 936 | -1.001700 -3.269130 1.037756 0 0 -1.588645 937 | 938 | 939 | 940 | 941 | model://aws_robomaker_retail_Holder_01 942 | 943 | -5.929110 -3.032790 1.037756 0 -0 1.550000 944 | 945 | 946 | 947 | 948 | model://aws_robomaker_retail_Holder_01 949 | 950 | -5.929110 -4.603640 1.037756 0 -0 1.550000 951 | 952 | 953 | 954 | 955 | model://aws_robomaker_retail_Horologe_01 956 | 957 | 2.405124 3.025708 2.525191 0 -0 -1.565582 958 | 959 | 960 | 961 | 962 | 963 | 964 | 965 | 966 | model://aws_robomaker_retail_RetailShopWall_01 967 | 968 | 0 0 -0.281282 0 -0 0 969 | 970 | 971 | 972 | 973 | model://aws_robomaker_retail_LightA_01 974 | 975 | 2.774418 -3.628150 3.150329 0 -0 0 976 | 977 | 978 | 979 | 980 | model://aws_robomaker_retail_LightA_01 981 | 982 | 5.383991 -5.383710 3.150328 0 -0 0 983 | 984 | 985 | 986 | 987 | model://aws_robomaker_retail_LightA_01 988 | 989 | 5.383991 0.041920 3.150328 0 -0 0 990 | 991 | 992 | 993 | 994 | model://aws_robomaker_retail_LightA_01 995 | 996 | -3.210668 -5.383710 3.150328 0 -0 0 997 | 998 | 999 | 1000 | 1001 | model://aws_robomaker_retail_LightA_01 1002 | 1003 | -3.271001 2.685925 3.150328 0 -0 0 1004 | 1005 | 1006 | 1007 | 1008 | model://aws_robomaker_retail_MagicCube_01 1009 | 1010 | 5.667560 -5.174908 0.968597 1.559939 -0.000001 -1.555409 1011 | 1012 | 1013 | 1014 | 1015 | model://aws_robomaker_retail_MagicCube_01 1016 | 1017 | -5.383672 5.177522 0.968608 1.564128 -0.000001 -1.555410 1018 | 1019 | 1020 | 1021 | 1022 | model://aws_robomaker_retail_MagicCube_01 1023 | 1024 | -3.146512 2.928458 1.035654 0 0 0 1025 | 1026 | 1027 | 1028 | 1029 | 1030 | 1031 | model://aws_robomaker_retail_Spotlight_01 1032 | 1033 | 1.493200 4.692372 3.302658 0 -0 0 1034 | 1035 | 1036 | 1037 | 1038 | model://aws_robomaker_retail_ShopWindow_01 1039 | 1040 | -7.738156 0.015960 -0.180092 0 -0 0 1041 | 1042 | 1043 | 1044 | 1045 | 1059 | 1060 | 1061 | 2.774418 -3.628150 3.178466 0 -0 0 1062 | 0.5 0.5 0.5 1 1063 | 0.1 0.1 0.1 1 1064 | 1065 | 20 1066 | 0.32 1067 | 0.01 1068 | 0.03 1069 | 1070 | 0 1071 | 0 0 -1 1072 | 1073 | 1074 | 5.383991 -5.383710 3.178466 0 -0 0 1075 | 0.5 0.5 0.5 1 1076 | 0.1 0.1 0.1 1 1077 | 1078 | 20 1079 | 0.32 1080 | 0.01 1081 | 0.03 1082 | 1083 | 0 1084 | 0 0 -1 1085 | 1086 | 1087 | 5.383991 0.041920 3.178466 0 -0 0 1088 | 0.5 0.5 0.5 1 1089 | 0.1 0.1 0.1 1 1090 | 1091 | 20 1092 | 0.32 1093 | 0.01 1094 | 0.03 1095 | 1096 | 0 1097 | 0 0 -1 1098 | 1099 | 1100 | -3.210668 -5.383710 3.178466 0 -0 0 1101 | 0.5 0.5 0.5 1 1102 | 0.1 0.1 0.1 1 1103 | 1104 | 20 1105 | 0.32 1106 | 0.01 1107 | 0.03 1108 | 1109 | 0 1110 | 0 0 -1 1111 | 1112 | 1113 | -3.271001 2.685925 3.178466 0 -0 0 1114 | 0.5 0.5 0.5 1 1115 | 0.1 0.1 0.1 1 1116 | 1117 | 20 1118 | 0.32 1119 | 0.01 1120 | 0.03 1121 | 1122 | 0 1123 | 0 0 -1 1124 | 1125 | 1126 | 1.493200 4.692372 3.178466 0 -0 0 1127 | 0.5 0.5 0.5 1 1128 | 0.1 0.1 0.1 1 1129 | 1130 | 20 1131 | 0.32 1132 | 0.01 1133 | 0.03 1134 | 1135 | 0 1136 | 0 0 -1 1137 | 1138 | 1139 | 1140 | --------------------------------------------------------------------------------