Test spring

CMakeLists.txt

@@ -16,20 +16,23 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(Eigen3 REQUIRED)
+find_package(Lua 5.4 REQUIRED)
+find_package(PCL REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(pcl_conversions REQUIRED)
+
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
-find_package(nav2_msgs REQUIRED)
-find_package(PCL REQUIRED)
 
+find_package(rmcs_executor REQUIRED)
+
+find_package(nav2_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
-find_package(std_srvs REQUIRED)
 find_package(rmcs_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(tf2_ros REQUIRED)
-find_package(Lua 5.4 REQUIRED)
-
-find_package(rmcs_executor REQUIRED)
 
 include(FetchContent)
 FetchContent_Declare(
@@ -49,14 +52,17 @@ target_compile_features(
 target_sources(
     ${PROJECT_NAME}
     PRIVATE
+        # src/cxx/util/localization/engine.cc
+        src/cxx/util/navigation/navigation.cc
         src/cxx/context.cc
-        src/cxx/navigation.cc
         src/cxx/component.cc
 )
+set(RMCS_CORE_SOURCE_DIR "${PROJECT_SOURCE_DIR}/../../rmcs_core/src")
 target_include_directories(
     ${PROJECT_NAME}
-    PRIVATE
+    SYSTEM PRIVATE
         ${PROJECT_SOURCE_DIR}/src/
+        ${RMCS_CORE_SOURCE_DIR}
         ${rmcs_executor_INCLUDE_DIRS}
         ${geometry_msgs_INCLUDE_DIRS}
         ${rmcs_msgs_INCLUDE_DIRS}
@@ -65,7 +71,10 @@ target_include_directories(
         ${tf2_ros_INCLUDE_DIRS}
         ${rclcpp_action_INCLUDE_DIRS}
         ${nav2_msgs_INCLUDE_DIRS}
+        ${sensor_msgs_INCLUDE_DIRS}
+        ${pcl_conversions_INCLUDE_DIRS}
         ${LUA_INCLUDE_DIR}
+        ${PCL_INCLUDE_DIRS}
 )
 target_link_libraries(
     ${PROJECT_NAME}
@@ -84,6 +93,33 @@ target_link_libraries(
         ${tf2_ros_LIBRARIES}
         ${rclcpp_action_LIBRARIES}
         ${nav2_msgs_LIBRARIES}
+        ${sensor_msgs_LIBRARIES}
+        ${pcl_conversions_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME}-sim-sidecar src/cxx/sim_sidecar.cc)
+target_compile_features(
+    ${PROJECT_NAME}-sim-sidecar
+    PRIVATE
+        cxx_std_23
+)
+target_compile_definitions(
+    ${PROJECT_NAME}-sim-sidecar
+    PRIVATE
+        RMCS_NAVIGATION_SOURCE_DIR="${PROJECT_SOURCE_DIR}"
+)
+target_include_directories(
+    ${PROJECT_NAME}-sim-sidecar
+    PRIVATE
+        ${PROJECT_SOURCE_DIR}/src/
+        ${LUA_INCLUDE_DIR}
+)
+target_link_libraries(
+    ${PROJECT_NAME}-sim-sidecar
+    PRIVATE
+        ${LUA_LIBRARIES}
+        sol2::sol2
+        yaml-cpp::yaml-cpp
 )
 
 find_package(pluginlib REQUIRED)

cmake/export.cmake

@@ -1,5 +1,5 @@
 install(
-    TARGETS ${PROJECT_NAME}
+    TARGETS ${PROJECT_NAME} ${PROJECT_NAME}-sim-sidecar
     DESTINATION lib/${PROJECT_NAME}/
 )
 install(

config/motion.yaml

@@ -106,32 +106,32 @@ controller_server:
           "PathAlignCritic",
           "PathFollowCritic",
         ]
-
-      ConstraintCritic.enabled: true
-      ConstraintCritic.cost_weight: 5.0
-
-      CostCritic.enabled: true
-      CostCritic.cost_weight: 16.0
-      CostCritic.critical_cost: 120.0
-      CostCritic.collision_cost: 8000000.0
-      CostCritic.near_collision_cost: 253
-      CostCritic.cost_power: 2
-      CostCritic.consider_footprint: true
-      CostCritic.near_goal_distance: 0.8
-      CostCritic.trajectory_point_step: 1
-
-      GoalCritic.enabled: true
-      GoalCritic.cost_weight: 4.5
-      GoalCritic.threshold_to_consider: 1.2
-
-      PathAlignCritic.enabled: true
-      PathAlignCritic.cost_weight: 0.8
-      PathAlignCritic.threshold_to_consider: 1.0
-      PathAlignCritic.trajectory_point_step: 3
-
-      PathFollowCritic.enabled: true
-      PathFollowCritic.cost_weight: 3.2
-      PathFollowCritic.threshold_to_consider: 1.5
+      ConstraintCritic:
+        enabled: true
+        cost_weight: 5.0
+      CostCritic:
+        enabled: true
+        cost_weight: 10.0
+        critical_cost: 120.0
+        collision_cost: 8000000.0
+        near_collision_cost: 230
+        cost_power: 2
+        consider_footprint: true
+        near_goal_distance: 1.0
+        trajectory_point_step: 1
+      GoalCritic:
+        enabled: true
+        cost_weight: 10.0
+        threshold_to_consider: 1.0
+      PathAlignCritic:
+        enabled: true
+        cost_weight: 0.8
+        threshold_to_consider: 1.0
+        trajectory_point_step: 3
+      PathFollowCritic:
+        enabled: true
+        cost_weight: 3.2
+        threshold_to_consider: 1.5
 
 local_costmap:
   local_costmap:
@@ -162,5 +162,5 @@ local_costmap:
 
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        inflation_radius: 0.25
+        inflation_radius: 0.4
         cost_scaling_factor: 0.5

godot-mock/.editorconfig

@@ -0,0 +1,4 @@
+root = true
+
+[*]
+charset = utf-8

godot-mock/.gitattributes

@@ -0,0 +1,2 @@
+# Normalize EOL for all files that Git considers text files.
+* text=auto eol=lf

godot-mock/.gitignore

@@ -0,0 +1,3 @@
+# Godot 4+ specific ignores
+.godot/
+/android/

godot-mock/material/20260422-192911.png.import

@@ -0,0 +1,41 @@
+[remap]
+
+importer="texture"
+type="CompressedTexture2D"
+uid="uid://dqtsgnct0ijl0"
+path.s3tc="res://.godot/imported/20260422-192911.png-f31ee39485589f6eb4a8fd243ef62172.s3tc.ctex"
+metadata={
+"imported_formats": ["s3tc_bptc"],
+"vram_texture": true
+}
+
+[deps]
+
+source_file="res://material/20260422-192911.png"
+dest_files=["res://.godot/imported/20260422-192911.png-f31ee39485589f6eb4a8fd243ef62172.s3tc.ctex"]
+
+[params]
+
+compress/mode=2
+compress/high_quality=false
+compress/lossy_quality=0.7
+compress/uastc_level=0
+compress/rdo_quality_loss=0.0
+compress/hdr_compression=1
+compress/normal_map=0
+compress/channel_pack=0
+mipmaps/generate=true
+mipmaps/limit=-1
+roughness/mode=0
+roughness/src_normal=""
+process/channel_remap/red=0
+process/channel_remap/green=1
+process/channel_remap/blue=2
+process/channel_remap/alpha=3
+process/fix_alpha_border=true
+process/premult_alpha=false
+process/normal_map_invert_y=false
+process/hdr_as_srgb=false
+process/hdr_clamp_exposure=false
+process/size_limit=0
+detect_3d/compress_to=0

godot-mock/material/Cn_Mat_U_LMSuit_21_body_BIANSE.tres

@@ -0,0 +1,7 @@
+[gd_resource type="StandardMaterial3D" format=3 uid="uid://bnfxtpyuftji1"]
+
+[resource]
+resource_name = "Cn_Mat_U_LMSuit_21_body_BIANSE"
+cull_mode = 2
+albedo_color = Color(0.9063318, 0.9063318, 0.9063318, 1)
+roughness = 0.9090847

godot-mock/material/white_wood.png.import

@@ -0,0 +1,41 @@
+[remap]
+
+importer="texture"
+type="CompressedTexture2D"
+uid="uid://dodhbatwmlp07"
+path.s3tc="res://.godot/imported/white_wood.png-a533a54625f96f5b178df59798e15a83.s3tc.ctex"
+metadata={
+"imported_formats": ["s3tc_bptc"],
+"vram_texture": true
+}
+
+[deps]
+
+source_file="res://material/white_wood.png"
+dest_files=["res://.godot/imported/white_wood.png-a533a54625f96f5b178df59798e15a83.s3tc.ctex"]
+
+[params]
+
+compress/mode=2
+compress/high_quality=false
+compress/lossy_quality=0.7
+compress/uastc_level=0
+compress/rdo_quality_loss=0.0
+compress/hdr_compression=1
+compress/normal_map=0
+compress/channel_pack=0
+mipmaps/generate=true
+mipmaps/limit=-1
+roughness/mode=0
+roughness/src_normal=""
+process/channel_remap/red=0
+process/channel_remap/green=1
+process/channel_remap/blue=2
+process/channel_remap/alpha=3
+process/fix_alpha_border=true
+process/premult_alpha=false
+process/normal_map_invert_y=false
+process/hdr_as_srgb=false
+process/hdr_clamp_exposure=false
+process/size_limit=0
+detect_3d/compress_to=0

godot-mock/models/Cn_Mat_U_LMSuit_21_body_BIANSE.tres

@@ -0,0 +1,7 @@
+[gd_resource type="StandardMaterial3D" format=3 uid="uid://8d75nvfc3dnj"]
+
+[resource]
+resource_name = "Cn_Mat_U_LMSuit_21_body_BIANSE"
+cull_mode = 2
+albedo_color = Color(0.9063318, 0.9063318, 0.9063318, 1)
+roughness = 0.9090847

godot-mock/models/cube.glb.import

@@ -0,0 +1,50 @@
+[remap]
+
+importer="scene"
+importer_version=1
+type="PackedScene"
+uid="uid://iisq5ccb4jt1"
+path="res://.godot/imported/cube.glb-c6bf6ebacd621473d1ca4ff4a368b9dc.scn"
+
+[deps]
+
+source_file="res://models/cube.glb"
+dest_files=["res://.godot/imported/cube.glb-c6bf6ebacd621473d1ca4ff4a368b9dc.scn"]
+
+[params]
+
+nodes/root_type="Node3D"
+nodes/root_name="Cube"
+nodes/root_script=null
+nodes/apply_root_scale=true
+nodes/root_scale=1.0
+nodes/import_as_skeleton_bones=false
+nodes/use_name_suffixes=true
+nodes/use_node_type_suffixes=true
+meshes/ensure_tangents=true
+meshes/generate_lods=true
+meshes/create_shadow_meshes=true
+meshes/light_baking=0
+meshes/lightmap_texel_size=0.1
+meshes/force_disable_compression=false
+skins/use_named_skins=true
+animation/import=false
+animation/fps=15
+animation/trimming=false
+animation/remove_immutable_tracks=true
+animation/import_rest_as_RESET=false
+import_script/path=""
+materials/extract=0
+materials/extract_format=0
+materials/extract_path=""
+_subresources={
+"materials": {
+"cube_material": {
+"use_external/enabled": true,
+"use_external/fallback_path": "res://models/cube_material.tres",
+"use_external/path": "uid://cbwgleagikdfb"
+}
+}
+}
+gltf/naming_version=2
+gltf/embedded_image_handling=1

godot-mock/models/cube_material.tres

@@ -0,0 +1,9 @@
+[gd_resource type="StandardMaterial3D" format=3 uid="uid://cbwgleagikdfb"]
+
+[ext_resource type="Texture2D" uid="uid://dodhbatwmlp07" path="res://material/white_wood.png" id="1"]
+
+[resource]
+resource_name = "Material"
+albedo_color = Color(0, 0, 0, 1)
+albedo_texture = ExtResource("1")
+roughness = 0.75

godot-mock/models/d7a3f20de4e4486182806c40699d26d4.glb.import

@@ -0,0 +1,42 @@
+[remap]
+
+importer="scene"
+importer_version=1
+type="PackedScene"
+uid="uid://bnx0kbsvbubto"
+path="res://.godot/imported/d7a3f20de4e4486182806c40699d26d4.glb-dbd1b1a6503096bd82aa2dd7e4f4cc15.scn"
+
+[deps]
+
+source_file="res://models/d7a3f20de4e4486182806c40699d26d4.glb"
+dest_files=["res://.godot/imported/d7a3f20de4e4486182806c40699d26d4.glb-dbd1b1a6503096bd82aa2dd7e4f4cc15.scn"]
+
+[params]
+
+nodes/root_type=""
+nodes/root_name=""
+nodes/root_script=null
+nodes/apply_root_scale=true
+nodes/root_scale=1.0
+nodes/import_as_skeleton_bones=false
+nodes/use_name_suffixes=true
+nodes/use_node_type_suffixes=true
+meshes/ensure_tangents=true
+meshes/generate_lods=true
+meshes/create_shadow_meshes=true
+meshes/light_baking=0
+meshes/lightmap_texel_size=0.2
+meshes/force_disable_compression=false
+skins/use_named_skins=true
+animation/import=true
+animation/fps=30
+animation/trimming=false
+animation/remove_immutable_tracks=true
+animation/import_rest_as_RESET=false
+import_script/path=""
+materials/extract=1
+materials/extract_format=0
+materials/extract_path=""
+_subresources={}
+gltf/naming_version=2
+gltf/embedded_image_handling=1