Luca/phase 2 engine

aic_engine/CMakeLists.txt

@@ -13,6 +13,7 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(intrinsic_sdk_cmake REQUIRED)
 
 find_package(aic_scoring REQUIRED)
 find_package(aic_control_interfaces REQUIRED)
@@ -24,26 +25,45 @@ find_package(std_srvs REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
+intrinsic_sdk_generate_service_manifest(
+  SERVICE_NAME ${PROJECT_NAME}
+  MANIFEST ${PROJECT_NAME}.manifest.textproto
+  PARAMETER_DESCRIPTOR ${PROJECT_NAME}.proto
+  DEFAULT_CONFIGURATION ${PROJECT_NAME}_default_config.pbtxt
+  PROTOS_TARGET ${PROJECT_NAME}_protos
+)
+
 add_executable(aic_engine
   src/aic_engine.cpp
-	src/main.cpp
+  src/main.cpp
+)
+target_include_directories(aic_engine PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>
 )
 target_link_libraries(aic_engine
   aic_scoring::aic_scoring
   rclcpp::rclcpp
   rclcpp_action::rclcpp_action
-  yaml-cpp
+  yaml-cpp::yaml-cpp
   ${aic_control_interfaces_TARGETS}
   ${aic_engine_interfaces_TARGETS}
   ${lifecycle_msgs_TARGETS}
   ${std_srvs_TARGETS}
+  ${PROJECT_NAME}_protos
+  intrinsic_sdk_cmake::intrinsic_sdk_cmake
 )
 
 install(
   TARGETS aic_engine
   DESTINATION lib/${PROJECT_NAME}
 )
 
+# Install proto descriptor file
+install(FILES
+  ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_protos.desc
+  DESTINATION share/${PROJECT_NAME}
+)
+
 # Install directories.
 install(
   DIRECTORY config

flowstate/services/aic_engine/aic_engine.manifest.textproto → aic_engine/aic_engine.manifest.textproto

@@ -21,8 +21,10 @@ service_def {
     image {
       archive_filename: "aic_engine.tar"
     }
-  } 
+  }
 }
 assets {
+  default_configuration_filename: "default_config.binarypb",
+  parameter_descriptor_filename: "parameter-descriptor-set.proto.bin",
   image_filenames: ["aic_engine.tar"]
 }

aic_engine/aic_engine.proto

@@ -0,0 +1,7 @@
+syntax = "proto3";
+
+package intrinsic;
+
+message AicEngineConfig {
+  bool use_sim_time = 1;
+}

aic_engine/aic_engine_default_config.pbtxt

@@ -0,0 +1,6 @@
+# proto-file: aic_engine.proto
+# proto-message: AicEngineConfig
+# Corresponds to the message definition in aic_engine.proto
+[type.googleapis.com/intrinsic.AicEngineConfig] {
+  use_sim_time: true
+}

aic_engine/package.xml

@@ -12,6 +12,7 @@
   <depend>aic_scoring</depend>
   <depend>aic_control_interfaces</depend>
   <depend>aic_engine_interfaces</depend>
+  <depend>intrinsic_sdk_cmake</depend>
   <depend>lifecycle_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>

aic_engine/pixi.toml

@@ -0,0 +1,21 @@
+[package.build.backend]
+name = "pixi-build-ros"
+version = "==0.3.3.20260113.c8b6a54"
+channels = [
+  "https://prefix.dev/pixi-build-backends",
+  "robostack-kilted",
+  "conda-forge",
+]
+
+[package.host-dependencies]
+ros-kilted-aic-control-interfaces = { path = "../aic_interfaces/aic_control_interfaces" }
+ros-kilted-aic-task-interfaces = { path = "../aic_interfaces/aic_task_interfaces" }
+ros-kilted-aic-engine-interfaces= { path = "../aic_interfaces/aic_engine_interfaces" }
+ros-kilted-aic-scoring = { path = "../aic_scoring" }
+
+[package.build-dependencies]
+ros-kilted-aic-control-interfaces = { path = "../aic_interfaces/aic_control_interfaces" }
+ros-kilted-aic-task-interfaces = { path = "../aic_interfaces/aic_task_interfaces" }
+ros-kilted-aic-engine-interfaces= { path = "../aic_interfaces/aic_engine_interfaces" }
+ros-kilted-aic-scoring = { path = "../aic_scoring" }
+

aic_engine/src/aic_engine.cpp

@@ -460,8 +460,11 @@ bool Engine::check_endpoints() {
 bool Engine::ready_scoring(const uint64_t time_limit) {
   RCLCPP_INFO(node_->get_logger(), "Checking scoring system readiness...");
 
+  const bool use_sim_time = node_->get_parameter_or("use_sim_time", true);
+
   // Add a few seconds for safety since this is a limit for recorded data
-  if (!scoring_tier2_->StartRecording(std::chrono::seconds(time_limit + 10))) {
+  if (!scoring_tier2_->StartRecording(std::chrono::seconds(time_limit + 10),
+                                      use_sim_time)) {
     RCLCPP_ERROR(node_->get_logger(), "Failed to start recording");
     return false;
   }
@@ -480,8 +483,7 @@ void Engine::compute_score(TrialScore& score) {
   score.tier_2 = tier2_score;
   score.tier_3 = tier3_score;
 
-  RCLCPP_INFO(node_->get_logger(), "Finished scoring trial, total score is: %f",
-              score.total_score());
+  RCLCPP_INFO(node_->get_logger(),"Finished scoring trial. Total score will be provided by AIC Team");
 
   YAML::Node yaml_score;
   yaml_score["tier_1"] = score.tier_1.to_yaml();

aic_engine/src/main.cpp

@@ -16,15 +16,50 @@
  */
 
 #include <cstdlib>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <vector>
 
 #include "aic_engine.hpp"
+#include "aic_engine.pb.h"
+#include "intrinsic/resources/proto/runtime_context.pb.h"
 #include "rclcpp/executor.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+//==============================================================================
+intrinsic_proto::config::RuntimeContext GetRuntimeContext() {
+  intrinsic_proto::config::RuntimeContext runtime_context;
+  std::ifstream runtime_context_file;
+  runtime_context_file.open("/etc/intrinsic/runtime_config.pb",
+                            std::ios::binary);
+  if (!runtime_context.ParseFromIstream(&runtime_context_file)) {
+    // Return default context for running locally
+    std::cerr << "Warning: using default RuntimeContext\n";
+  }
+  return runtime_context;
+}
 
 //==============================================================================
 int main(int argc, char** argv) {
+  auto runtime_context = GetRuntimeContext();
+  intrinsic::AicEngineConfig config;
+  bool use_sim_time = true;
+  if (runtime_context.config().UnpackTo(&config)) {
+    use_sim_time = config.use_sim_time();
+  } else {
+    std::cerr << "Warning: Error unpacking AicEngineConfig from service "
+                 "config file... Passing default ros args to node\n";
+  }
+
   rclcpp::init(argc, argv);
 
-  auto engine = std::make_shared<aic::Engine>();
+  rclcpp::NodeOptions options;
+  std::vector<rclcpp::Parameter> params;
+  params.emplace_back("use_sim_time", use_sim_time);
+  options.parameter_overrides(params);
+
+  auto engine = std::make_shared<aic::Engine>(options);
   engine->spin();
 
   rclcpp::shutdown();

aic_interfaces/aic_control_interfaces/pixi.toml

@@ -6,3 +6,9 @@ channels = [
   "robostack-kilted",
   "conda-forge",
 ]
+
+[package.build-dependencies]
+empy = "==3.3.4"
+
+[package.host-dependencies]
+empy = "==3.3.4"

aic_interfaces/aic_engine_interfaces/pixi.toml

@@ -0,0 +1,14 @@
+[package.build.backend]
+name = "pixi-build-ros"
+version = "==0.3.3.20260113.c8b6a54"
+channels = [
+  "https://prefix.dev/pixi-build-backends",
+  "robostack-kilted",
+  "conda-forge",
+]
+
+[package.build-dependencies]
+empy = "==3.3.4"
+
+[package.host-dependencies]
+empy = "==3.3.4"

aic_interfaces/aic_model_interfaces/pixi.toml

@@ -8,7 +8,9 @@ channels = [
 ]
 
 [package.host-dependencies]
+empy = "==3.3.4"
 ros-kilted-aic-control-interfaces = { path = "../aic_control_interfaces" }
 
 [package.build-dependencies]
+empy = "==3.3.4"
 ros-kilted-aic-control-interfaces = { path = "../aic_control_interfaces" }

aic_interfaces/aic_task_interfaces/pixi.toml

@@ -6,3 +6,9 @@ channels = [
   "robostack-kilted",
   "conda-forge",
 ]
+
+[package.build-dependencies]
+empy = "==3.3.4"
+
+[package.host-dependencies]
+empy = "==3.3.4"

aic_scoring/include/aic_scoring/ScoringTier2.hh

@@ -150,7 +150,9 @@ namespace aic_scoring
     /// \brief Start recording all scoring topics.
     /// \return True if the scoring system is ready.
     /// \param[in] _max_task_time The maximum time to record for, used for tf buffer size.
-    public: bool StartRecording(const std::chrono::seconds &_max_task_time);
+    /// \param[in] _use_sim_time Whether to wait for simulation TFs.
+    public: bool StartRecording(const std::chrono::seconds &_max_task_time,
+                                const bool _use_sim_time = true);
 
     /// \brief Stop recording all scoring topics.
     /// \return True if the bag was closed correctly.
@@ -210,16 +212,12 @@ namespace aic_scoring
     private: void ControllerStateCallback(const ControllerStateMsg& _msg);
 
     /// \brief Calculates score related with the gripper trajectory jerk.
-    /// \param[in] _success Whether the task succeeded.
     /// \return Scoring for the trajectory jerk score.
-    private: Tier2Score::CategoryScore GetTrajectoryJerkScore(
-        const bool _success) const;
+    private: Tier2Score::CategoryScore GetTrajectoryJerkScore() const;
 
     /// \brief Calculates score for trajectory efficiency (path length).
-    /// \param[in] _success Whether the task succeeded.
     /// \return Scoring for the trajectory efficiency category.
-    private: Tier2Score::CategoryScore GetTrajectoryEfficiencyScore(
-        const bool _success) const;
+    private: Tier2Score::CategoryScore GetTrajectoryEfficiencyScore() const;
 
     /// \brief Gets the transform for the specified entity at the requested time.
     /// \param[in] _t the time point to get the transform.
@@ -258,19 +256,16 @@ namespace aic_scoring
     private: Tier3Score ComputeTier3Score(std::size_t index) const;
 
     /// \brief Compute and combine tier 3 scores.
-    /// \return The tier3 score for the whole cable and a boolean that is true
-    /// if both tasks were close to succeeding, false otherwise.
-    private: std::pair<bool, Tier3Score> CombineTier3Score() const;
+    /// \return The tier3 score for the whole cable.
+    private: Tier3Score CombineTier3Score() const;
 
     /// \brief Calculates the penalty (if any) for contacts with off limit entities.
     /// \return Scoring for the off limit contacts category
     private: Tier2Score::CategoryScore GetContactsScore() const;
 
     /// \brief Calculates the score for task duration.
-    /// \param[in] _success Whether the task succeeded.
     /// \return Scoring for the task duration category.
-    private: Tier2Score::CategoryScore GetTaskDurationScore(
-        const bool _success) const;
+    private: Tier2Score::CategoryScore GetTaskDurationScore() const;
 
     /// \brief Wait for the cable and gripper TFs to be received.
     /// \return True if the transform were received, false if timeout occurred.

aic_scoring/pixi.toml

@@ -0,0 +1,15 @@
+[package.build.backend]
+name = "pixi-build-ros"
+version = "==0.3.3.20260113.c8b6a54"
+channels = [
+  "https://prefix.dev/pixi-build-backends",
+  "robostack-kilted",
+  "conda-forge",
+]
+
+[package.host-dependencies]
+ros-kilted-aic-control-interfaces = { path = "../aic_interfaces/aic_control_interfaces" }
+
+[package.build-dependencies]
+ros-kilted-aic-control-interfaces = { path = "../aic_interfaces/aic_control_interfaces" }
+