Multi agent sampling

.github/workflows/docker-image.yml

@@ -2,7 +2,6 @@ name: Docker Image CI
 
 on:
   push:
-    branches: [ "master", "docker" ]
   pull_request:
     branches: [ "master" ]
 

active_3d_planning_app_reconstruction/CMakeLists.txt

@@ -9,6 +9,9 @@ add_definitions(-std=c++17)
 
 catkin_package()
 
+catkin_install_python(PROGRAMS scripts/planner_goal_pose_to_transform.py
+                         DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
 cs_add_executable(eval_voxblox_node
         src/experiments/eval_voxblox_node.cpp
         )

active_3d_planning_app_reconstruction/cfg/planners/example_config_rrt_star_anymal.yaml

@@ -1,112 +1,117 @@
-# General parameters for the planner node
-replan_pos_threshold: 0.4
-replan_yaw_threshold: 0.05
-
-max_new_segments: 0
-min_new_segments: 0
-max_new_tries: 0
-min_new_tries: 0
-min_new_value: 0
-expand_batch:  10
-
-# Verbosity and visualization
-verbose: true
-verbose_modules: true
-build_modules_on_init: true
-visualize: true
-highlight_executed_trajectory: true
-log_performance: true
-performance_log_dir: "/home/jiaqchen/sutd_julia_ws/logs/mav_active_3d_planning/specific"
-
-# Specify a map representation
-map:
-  type: "VoxbloxMap"
-
-# System constraints
-system_constraints:
-  v_max: 1.0
-  a_max: 1.0
-  yaw_rate_max: 1.6
-  yaw_accel_max: 1.6
-  collision_radius: 0.5
-
-trajectory_generator:
-  collision_optimistic: false
-  bounding_volume_args: "/map_bounding_volume"
-  clearing_radius: 2.0
-
-  type: "RRTStar"
-  all_semgents_feasible: false
-  crop_segments: true
-  crop_margin: 0.3
-  crop_min_length: 0.5
-  min_path_length: 0.5
-  max_extension_range: 1.5
-  maximum_tries: 1000
-  sampling_rate: 20.0
-  sample_yaw: true
-
-  sampling_mode: "semilocal"
-  semilocal_sampling_count: 10
-  semilocal_sampling_radius_max: 1.5
-  semilocal_sampling_radius_min: 1.5
-
-  rewire_root: true
-  rewire_intermediate: true
-  reinsert_root: true
-  max_rewire_range: 1.6
-  max_density_range: 1.0
-  n_neighbors: 20
-
-  generator_updater:
-    type: "RecheckCollision"
-
-trajectory_evaluator:
-  type: "RRTStarEvaluatorAdapter"
-
-  following_evaluator:
-    type: "ContinuousYawPlanningEvaluator"
-    n_directions: 12
-    n_sections_fov: 3
-    update_range: 3.0
-    update_gain: 0.0
-    update_sections_separate: false
-
-    following_evaluator:
-      type: "NaiveEvaluator"
-      visualize_sensor_view: true
-      bounding_volume_args: "/target_bounding_volume"
-
-      sensor_model:
-        type: "IterativeRayCaster"
-        sampling_time: 0.0
-        ray_length: 5.0
-        focal_length: 400.0
-        resolution_x: 640 
-        resolution_y: 480
-        downsampling_factor: 5.0
-        mounting_translation_x: 0.0 
-        mounting_translation_y: 0.0
-        mounting_translation_z: 0.0
-        mounting_rotation_x: 0
-        mounting_rotation_y: 0
-        mounting_rotation_z: 0
-        mounting_rotation_w: 1.0
-
-      cost_computer:
-        type: "SegmentTime"
-
-      value_computer:
-        type: "GlobalNormalizedGain"
-
-      next_selector:
-        type: "SubsequentBest"
-
-# Specify a backtracker if the planner gets stuck
-back_tracker:
-  type: "RotateReverse"
-  turn_rate: 1.6
-  update_rate: 2.0
-  sampling_rate: 20.0
-  n_rotations: 1
-  stack_size: 10
+# # General parameters for the planner node
+# replan_pos_threshold: 0.6
+# replan_yaw_threshold: 0.6
+
+# goal_topics: "/planner/command/trajectory_path/transform" # TODO: change for multi-agent
+
+# max_new_segments: 0
+# min_new_segments: 0
+# max_new_tries: 2
+# min_new_tries: 0
+# min_new_value: 0
+# expand_batch:  30
+
+# # Verbosity and visualization
+# verbose: true
+# verbose_modules: true
+# build_modules_on_init: true
+# visualize: true
+# highlight_executed_trajectory: true
+# log_performance: true
+# performance_log_dir: "/home/jiaqchen/sutd_julia_ws/logs/mav_active_3d_planning/specific"
+
+# # Specify a map representation
+# map:
+#   type: "VoxbloxMap"
+
+# # System constraints
+# system_constraints:
+#   v_max: 0.3
+#   a_max: 1.0
+#   yaw_rate_max: 0.3
+#   yaw_accel_max: 1.0
+#   collision_radius: 0.5
+
+# trajectory_generator:
+#   collision_optimistic: false
+#   bounding_volume_args: "/map_bounding_volume"
+#   clearing_radius: 2.0     # Unknown space within clearing radius is traversable
+
+#   type: "RRTStar"
+#   all_semgents_feasible: false
+#   crop_segments: true
+#   crop_margin: 0.2
+#   crop_min_length: 0.3
+#   min_path_length: 0.3
+#   max_extension_range: 0.0
+#   maximum_tries: 1000
+#   sampling_rate: 20.0
+#   sample_yaw: true
+#   robot_radius: 0.3
+#   keep_last_n: 5
+
+#   sampling_mode: "uniform"  #"semilocal"
+#   semilocal_sampling_count: 5
+#   semilocal_sampling_radius_max: 1.5
+#   semilocal_sampling_radius_min: 0.5
+
+#   rewire_root: true
+#   rewire_intermediate: true
+#   reinsert_root: true
+#   max_rewire_range: 3.0
+#   max_density_range: 1.0
+#   n_neighbors: 20
+
+#   generator_updater:
+#     type: "RecheckCollision"
+
+# trajectory_evaluator:
+#   type: "RRTStarEvaluatorAdapter"
+
+#   following_evaluator:
+#     type: "ContinuousYawPlanningEvaluator"
+#     n_directions: 12
+#     n_sections_fov: 3
+#     update_range: 3.0
+#     update_gain: 0.0
+#     update_sections_separate: false
+
+#     following_evaluator:
+#       type: "NaiveEvaluator"
+#       visualize_sensor_view: true
+#       bounding_volume_args: "/target_bounding_volume"
+
+#       sensor_model:
+#         type: "IterativeRayCaster"
+#         sampling_time: 0.0
+#         ray_length: 5.0
+#         focal_length: 400.0
+#         resolution_x: 640 
+#         resolution_y: 480
+#         downsampling_factor: 5.0
+#         mounting_translation_x: 0.0 
+#         mounting_translation_y: 0.0
+#         mounting_translation_z: 0.0
+#         mounting_rotation_x: 0
+#         mounting_rotation_y: 0
+#         mounting_rotation_z: 0
+#         mounting_rotation_w: 1.0
+
+
+#       cost_computer:
+#         type: "SegmentTime"
+
+#       value_computer:
+#         type: "GlobalNormalizedGain"
+
+#       next_selector:
+#         type: "SubsequentBest"
+
+# # Specify a backtracker if the planner gets stuck
+# back_tracker:
+#   type: "RotateReverse"
+#   turn_rate: 1.6
+#   update_rate: 2.0
+#   sampling_rate: 20.0
+#   n_rotations: 1
+#   stack_size: 10