Demo

.gitignore

@@ -0,0 +1 @@
+__pycache__/

ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml

@@ -2,9 +2,12 @@ octomap:
   octomap_topic: "/octomap_full"
   pointcloud_topic: "/$(arg name)/semantic_pcl"
   frame_id: "world"
+  # resolution: 0.3
+  # max_range: 10.0
+  # raycast_range: 0.5
   resolution: 0.3
-  max_range: 10.0
-  raycast_range: 0.5
+  max_range: 15.0
+  raycast_range: 10.0
   clamping_thres_min: 0.006692851
   clamping_thres_max: 0.993307149
   occupancy_thres: 0.5
@@ -13,25 +16,26 @@ octomap:
   psi: 1
   phi: -0.3
   publish_2d_map: true
-  min_ground_z: -1.5
-  max_ground_z: 1.5
-  consensus_weight: 0.8
-  sub_timer: 10
-  pub_timer: 10
+  min_ground_z: 0.0
+  max_ground_z: 0.7
+  consensus_weight: 0.2
+  sub_timer: 5
+  pub_timer: 5
 
 # Camera intrinsic matrix parameters
 camera:
-  fx: 65.24951330239097
-  fy: 65.24951330239097
-  cx: 80.0
-  cy: 60.0
+  fx: 320
+  fy: 320
+  cx: 320
+  cy: 240
 
-  width: 160
-  height: 120
+  width: 640
+  height: 480
 
 semantic_pcl:
-  color_image_topic: "/$(arg name)/camera/image"
-  semantic_image_topic: "/$(arg name)/camera/semantic_image"
-  depth_image_topic: "/$(arg name)/depth_camera/depth/image_rect_raw"
+  color_image_topic: "/$(arg name)/image"
+  semantic_image_topic: "/$(arg name)/colored_map"
+  depth_image_topic: "/$(arg name)/depth_image"
   point_type: 0
-  frame_id: "$(arg name)/camera"
+  frame_id: "$(arg name)/camera_optic"
+  unit_conversion: 1.0

ROAM-Mapping/src/octomap_generator_semantic_ros.cpp

@@ -166,7 +166,8 @@ void SemanticOctomapGeneratorNode::publish2DOccupancyMap(const SemanticOctree* o
   {
 
     double node_z = it.getZ();
-    double node_half_side = pow(it.getSize(), 1/3) / 2;
+    // double node_half_side = pow(it.getSize(), 1/3) / 2;
+    double node_half_side = it.getSize()/2;
     double top_side = node_z + node_half_side;
     double bottom_side = node_z - node_half_side;
 

ROAM-Planning/CMakeLists.txt

@@ -40,7 +40,7 @@ set(SOURCE_FILES
         src/cpp/src/exploration/util.cpp
         )
 
-set(EXPLORATION_LIB_DESTINATION "${CATKIN_DEVEL_PREFIX}/lib/python2.7/dist-packages")
+set(EXPLORATION_LIB_DESTINATION "${CATKIN_DEVEL_PREFIX}/lib/python3/dist-packages")
 add_library(exploration_cpp SHARED ${PROJECT_SOURCE_DIR} ${SOURCE_FILES} src/cpp/src/exploration/python.cpp)
 target_link_libraries(exploration_cpp ${PYTHON_LIBRARIES})
 set_target_properties(exploration_cpp PROPERTIES SUFFIX ".so" PREFIX "_"

ROAM-Planning/launch/planning_multi.launch

@@ -12,11 +12,11 @@
 		  <param name="agent_name" value="$(arg name)"/>
 		</node>
 
-		<!-- Optional: you can use this simple tracking and navigation node in order to execute the computed path.
+		<!-- Optional: you can use this simple tracking and navigation node in order to execute the computed path. -->
 		<node name="path_navigation_node" pkg="roam_planning" type="simple_path_navigation_node.py" output="screen" respawn="true">
 		  <param name="agent_name" value="$(arg name)"/>
 		</node>
-		-->
+
 
 	</group>
 

ROAM-Planning/params/multi_robot_exploration_params.yaml

@@ -1,40 +1,40 @@
 footprint:
-  type: 'tricky_circle'
+  type: 'jackal'
   angular_resolution: 0.01745
-  inflation_scale: 1.3
+  inflation_scale: 1.4
 
 planning:
-  SE2_radius: 16.0
-  max_iter: 5
-  max_path_length: 100
-  step_size: 0.001
-  angular_range: 1.570796327
+  SE2_radius: 2 
+  max_iter: 10 
+  max_path_length: 15.0
+  step_size: 0.001 
+  angular_range: 1.570796327 
   angular_samples: 32
   num_orientation_samples: 8
-  distance_coeff: 0.001
+  distance_coeff: 0.01 
   theta_coeff: 0.1
   min_frontier_size: 30
-  goal_check_radius: 2.0
-  draw_radius: 15
+  goal_check_radius: 0.3 
+  draw_radius: 0.2 
+  clean_kernel_size: 0.6 
+  erode_kernel_size: 0.8 
+  median_filter_enable: false 
   map_topic: '/$(arg name)/occupancy_map_2D'
   collision_check_period: 1
 
   multi:
-    num_agents: 6
-    horizon: 5
-    delta_q: 10.0
-    gamma_q: 0.01
-    robot_colision_coeff: 1000000
-    epsilon: 1
-    optimization_period: 0.1
-    should_plan_threshold: 0.3
-    need_plan_threshold: 0.2
-    need_plan_period: 1.0
+    num_agents: 2
+    horizon: 10 
+    delta_q: 10.0 
+    gamma_q: 0.01 
+    robot_colision_coeff: 5 
+    epsilon: 0.1 
+    should_plan_threshold: 0.6 
+    need_plan_threshold: 0.6 
+    need_plan_period: 0.05 
 
 navigation:
-  tau: 0.005
-  alpha: 0.999
-  radius: 2.0
-  dist: 1.0
-  safety_dist: 1.0
+  tau: 0.005 
+  dist: 0.2 
+  safety_dist: 0.1 
   publish_odom: true