fix: deleting static clock due to no sim_time feature (nasa#725)

behaviors/arm/src/arm_component.cc

@@ -491,18 +491,18 @@ class ArmComponent : public ff_util::FreeFlyerComponent {
     cfg_.AddFile("behaviors/arm.config");
     if (!cfg_.Initialize(nh))
       return AssertFault(ff_util::INITIALIZATION_FAILED,
-                  "Could not start config server");
+                  "Could not start config server", GetTimeNow());
 
     // Read the configuration for this specific node
     config_reader::ConfigReader *cfg = cfg_.GetConfigReader();
     config_reader::ConfigReader::Table joints;
     if (!cfg->GetTable(GetName().c_str(), &joints))
       return AssertFault(ff_util::INITIALIZATION_FAILED,
-                         "Cannot read LUA file");
+                         "Cannot read LUA file", GetTimeNow());
     std::string name;
     if (!joints.GetStr("pan", &name))
       return AssertFault(ff_util::INITIALIZATION_FAILED,
-                         "Cannot read PAN joint");
+                         "Cannot read PAN joint", GetTimeNow());
     joints_[PAN].name = name;
     joints_[PAN].generic = "pan";
     joints_[PAN].tol = cfg_.Get<double>("tol_pan");
@@ -511,7 +511,7 @@ class ArmComponent : public ff_util::FreeFlyerComponent {
     dictionary_[name] = PAN;
     if (!joints.GetStr("tilt", &name))
       return AssertFault(ff_util::INITIALIZATION_FAILED,
-                         "Cannot read TILT joint");
+                         "Cannot read TILT joint", GetTimeNow());
     joints_[TILT].name = name;
     joints_[TILT].generic = "tilt";
     joints_[TILT].tol = cfg_.Get<double>("tol_tilt");
@@ -520,7 +520,7 @@ class ArmComponent : public ff_util::FreeFlyerComponent {
     dictionary_[name] = TILT;
     if (!joints.GetStr("gripper", &name))
       return AssertFault(ff_util::INITIALIZATION_FAILED,
-                         "Cannot read GRIPPER joint");
+                         "Cannot read GRIPPER joint", GetTimeNow());
     joints_[GRIPPER].name = name;
     joints_[GRIPPER].generic = "gripper";
     joints_[GRIPPER].tol = cfg_.Get<double>("tol_gripper");

behaviors/dock/src/dock_component.cc

@@ -425,7 +425,7 @@ class DockComponent : public ff_util::FreeFlyerComponent {
     cfg_.AddFile("behaviors/dock.config");
     if (!cfg_.Initialize(nh))
       return AssertFault(ff_util::INITIALIZATION_FAILED,
-                        "Could not start config server");
+                        "Could not start config server", GetTimeNow());
 
     // One shot timer to check if we undock with a timeout
     timer_eps_.createTimer(cfg_.Get<double>("timeout_eps_response"),
@@ -498,13 +498,13 @@ class DockComponent : public ff_util::FreeFlyerComponent {
   // Timeout on a trajectory generation request
   void EnableTimeoutCallback(void) {
     return AssertFault(ff_util::INITIALIZATION_FAILED,
-                       "Could not find enable service");
+                       "Could not find enable service", GetTimeNow());
   }
 
   // Timeout on a trajectory generation request
   void UndockTimeoutCallback(void) {
     return AssertFault(ff_util::INITIALIZATION_FAILED,
-                       "Could not find undock service");
+                       "Could not find undock service", GetTimeNow());
   }
 
   // Ensure all clients are connected

behaviors/perch/src/perch_component.cc

@@ -497,7 +497,7 @@ class PerchComponent : public ff_util::FreeFlyerComponent {
     cfg_.AddFile("behaviors/perch.config");
     if (!cfg_.Initialize(nh))
       return AssertFault(ff_util::INITIALIZATION_FAILED,
-                         "Could not load config");
+                         "Could not load config", GetTimeNow());
 
     // Setup the platform name
     platform_name_ = GetPlatform();

localization/localization_manager/src/localization_manager_component.cc

@@ -100,25 +100,25 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
         // Turn on optical flow
         if (!OpticalFlow(curr_->second.RequiresOpticalFlow())) {
           AssertFault(ff_util::INITIALIZATION_FAILED,
-            "Could not initialize optical flow for the default pipeline");
+            "Could not initialize optical flow for the default pipeline", GetTimeNow());
           return STATE::DISABLED;
         }
         // Enable the pipeline
         if (!curr_->second.Toggle(true)) {
           AssertFault(ff_util::INITIALIZATION_FAILED,
-            "Could not enable teh default pipeline");
+            "Could not enable teh default pipeline", GetTimeNow());
           return STATE::DISABLED;
         }
         // Try and switch to the pipeline
         if (!SwitchFilterInput(curr_->second.GetMode())) {
           AssertFault(ff_util::INITIALIZATION_FAILED,
-            "Could not switch to the default pipeline");
+            "Could not switch to the default pipeline", GetTimeNow());
           return STATE::DISABLED;
         }
         // Now start using the new pipeline
         if (!curr_->second.Use(true)) {
           AssertFault(ff_util::INITIALIZATION_FAILED,
-            "Could not start using the default pipeline");
+            "Could not start using the default pipeline", GetTimeNow());
           return STATE::DISABLED;
         }
         // Now return the correct default state
@@ -139,7 +139,7 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
         // really do anything except issue a fault...
         if (curr_ == fall_) {
           AssertFault(ff_util::LOCALIZATION_PIPELINE_UNSTABLE,
-            "Currently on fallback pipeline and it is unstable");
+            "Currently on fallback pipeline and it is unstable", GetTimeNow());
           ResetTimer(timer_recovery_);
           return STATE::UNSTABLE;
         }
@@ -148,37 +148,37 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
         // Turn on optical flow
         if (fall_->second.RequiresOpticalFlow() && !OpticalFlow(true)) {
           AssertFault(ff_util::LOCALIZATION_PIPELINE_UNSTABLE,
-            "Could not toggle optical flow on begin");
+            "Could not toggle optical flow on begin", GetTimeNow());
           return STATE::DISABLED;
         }
         // Enable the pipeline
         if (!fall_->second.Toggle(true)) {
           AssertFault(ff_util::LOCALIZATION_PIPELINE_UNSTABLE,
-            "Could not enable fallback pipeline");
+            "Could not enable fallback pipeline", GetTimeNow());
           return STATE::DISABLED;
         }
         // Try and switch to the pipeline
         if (!SwitchFilterInput(fall_->second.GetMode())) {
           AssertFault(ff_util::LOCALIZATION_PIPELINE_UNSTABLE,
-            "Could not switch to fallback pipeline");
+            "Could not switch to fallback pipeline", GetTimeNow());
           return STATE::DISABLED;
         }
         // Now start using the new pipeline
         if (!fall_->second.Use(true)) {
           AssertFault(ff_util::INITIALIZATION_FAILED,
-            "Could not start using the default pipeline");
+            "Could not start using the default pipeline", GetTimeNow());
           return STATE::DISABLED;
         }
         // Disable the active the pipeline
         if (!curr_->second.Toggle(false)) {
           AssertFault(ff_util::LOCALIZATION_PIPELINE_UNSTABLE,
-            "Could not disable fallback pipeline");
+            "Could not disable fallback pipeline", GetTimeNow());
           return STATE::DISABLED;
         }
         // Turn off optical flow if it's not required
         if (!OpticalFlow(fall_->second.RequiresOpticalFlow())) {
           AssertFault(ff_util::LOCALIZATION_PIPELINE_UNSTABLE,
-            "Could not toggle optical flow on end");
+            "Could not toggle optical flow on end", GetTimeNow());
           return STATE::DISABLED;
         }
         // Set all pipelines to fallback
@@ -384,24 +384,24 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
     cfg_.AddFile("localization/localization_manager.config");
     if (!cfg_.Initialize(nh))
       return AssertFault(ff_util::INITIALIZATION_FAILED,
-                  "Could not start config server");
+                  "Could not start config server", GetTimeNow());
 
     // Read and configure the pipelines
     config_reader::ConfigReader::Table pipelines(cfg_.GetConfigReader(), "pipelines");
     for (int i = 0; i < pipelines.GetSize(); i++) {
       config_reader::ConfigReader::Table pipeline;
       if (!pipelines.GetTable(i + 1, &pipeline))
-        AssertFault(ff_util::INITIALIZATION_FAILED, "Could not get pipeline");
+        AssertFault(ff_util::INITIALIZATION_FAILED, "Could not get pipeline", GetTimeNow());
       // Get mandatory fields
       std::string id;
       if (!pipeline.GetStr("id", &id))
-        AssertFault(ff_util::INITIALIZATION_FAILED, "Could not get id");
+        AssertFault(ff_util::INITIALIZATION_FAILED, "Could not get id", GetTimeNow());
       std::string name;
       if (!pipeline.GetStr("name", &name))
-        AssertFault(ff_util::INITIALIZATION_FAILED, "Could not get name");
+        AssertFault(ff_util::INITIALIZATION_FAILED, "Could not get name", GetTimeNow());
       int mode;
       if (!pipeline.GetInt("ekf_input", &mode))
-        AssertFault(ff_util::INITIALIZATION_FAILED, "Could not get ekf_input");
+        AssertFault(ff_util::INITIALIZATION_FAILED, "Could not get ekf_input", GetTimeNow());
 
       // Allocate the pipeline
       Pipeline & p = pipelines_.emplace(id,
@@ -415,19 +415,19 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
             int max_confidence = 0;
             if (!pipeline.GetInt("max_confidence", &max_confidence)) {
               AssertFault(ff_util::INITIALIZATION_FAILED,
-                            "Could not get max confidence");
+                            "Could not get max confidence", GetTimeNow());
             }
 
             bool optical_flow = false;
             if (!pipeline.GetBool("optical_flow", &optical_flow)) {
               AssertFault(ff_util::INITIALIZATION_FAILED,
-                          "Could not get optical flow");
+                          "Could not get optical flow", GetTimeNow());
             }
 
             double timeout = 1.0;
             if (!pipeline.GetReal("timeout", &timeout)) {
               AssertFault(ff_util::INITIALIZATION_FAILED,
-                          "Could not get timeout");
+                          "Could not get timeout", GetTimeNow());
             }
 
             // Set a filter need
@@ -443,7 +443,7 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
           double enable_timeout = 1.0;
           if (!pipeline.GetReal("enable_timeout", &enable_timeout)) {
             AssertFault(ff_util::INITIALIZATION_FAILED,
-                        "Could not get enable_timeout");
+                        "Could not get enable_timeout", GetTimeNow());
           }
 
           // Set an enable need
@@ -458,7 +458,7 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
           double reg_timeout = 1.0;
           if (!pipeline.GetReal("reg_timeout", &reg_timeout)) {
             AssertFault(ff_util::INITIALIZATION_FAILED,
-                        "Could not get reg_timeout");
+                        "Could not get reg_timeout", GetTimeNow());
           }
 
           // Set an enable need
@@ -473,13 +473,13 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
           double feat_timeout = 1.0;
           if (!pipeline.GetReal("feat_timeout", &feat_timeout)) {
             AssertFault(ff_util::INITIALIZATION_FAILED,
-                        "Could not get feat_timeout");
+                        "Could not get feat_timeout", GetTimeNow());
           }
 
           int feat_threshold = 0;
           if (!pipeline.GetInt("feat_threshold", &feat_threshold)) {
             AssertFault(ff_util::INITIALIZATION_FAILED,
-                        "Could not get feat_threshold");
+                        "Could not get feat_threshold", GetTimeNow());
           }
 
           // Set a feature need
@@ -494,13 +494,13 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
           double depth_timeout = 1.0;
           if (!pipeline.GetReal("depth_timeout", &depth_timeout)) {
             AssertFault(ff_util::INITIALIZATION_FAILED,
-                        "Could not get depth_timeout");
+                        "Could not get depth_timeout", GetTimeNow());
           }
 
           int depth_threshold = 0;
           if (!pipeline.GetInt("depth_threshold", &depth_threshold)) {
             AssertFault(ff_util::INITIALIZATION_FAILED,
-                        "Could not get depth_threshold");
+                        "Could not get depth_threshold", GetTimeNow());
           }
 
           // Set a feature need
@@ -513,18 +513,18 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
     std::string pipeline;
     if (!cfg_.Get<std::string>("fallback", pipeline))
       AssertFault(ff_util::INITIALIZATION_FAILED,
-        "No valid fallback pipeline specfified in the localization manager");
+        "No valid fallback pipeline specfified in the localization manager", GetTimeNow());
     fall_ = pipelines_.find(pipeline);
     if (fall_ == pipelines_.end())
       AssertFault(ff_util::INITIALIZATION_FAILED,
-        "Fallback pipeline specified in config does not exist");
+        "Fallback pipeline specified in config does not exist", GetTimeNow());
     if (!cfg_.Get<std::string>("pipeline", pipeline))
       AssertFault(ff_util::INITIALIZATION_FAILED,
-        "No valid default pipeline specfified in the localization manager");
+        "No valid default pipeline specfified in the localization manager", GetTimeNow());
     curr_ = pipelines_.find(pipeline);
     if (curr_ == pipelines_.end())
       AssertFault(ff_util::INITIALIZATION_FAILED,
-        "Default pipeline specified in config does not exist");
+        "Default pipeline specified in config does not exist", GetTimeNow());
     goal_ = curr_;
 
     // Initialize all pipelines in the system
@@ -535,7 +535,7 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
         std::bind(&LocalizationManagerComponent::ConnectedCallback, this),
         std::bind(&LocalizationManagerComponent::TimeoutCallback, this))) {
         AssertFault(ff_util::INITIALIZATION_FAILED,
-          std::string("Could not init pipeline: ") + pipeline.first);
+          std::string("Could not init pipeline: ") + pipeline.first, GetTimeNow());
       }
     }
 
@@ -632,7 +632,7 @@ class LocalizationManagerComponent : public ff_util::FreeFlyerComponent {
   void TimeoutCallback() {
     FF_DEBUG_STREAM("TimeoutCallback()");
     AssertFault(ff_util::INITIALIZATION_FAILED,
-      "One of the manager or pipeline services failed to appear");
+      "One of the manager or pipeline services failed to appear", GetTimeNow());
   }
 
   // Called when a user manually updates the internal state

mobility/choreographer/include/choreographer/planner.h

@@ -223,7 +223,7 @@ class PlannerImplementation : public ff_util::FreeFlyerComponent {
   // called if action and service servers timeout on conenction.
   void InitFault(std::string const& msg ) {
     Error(msg);
-    AssertFault(ff_util::INITIALIZATION_FAILED, msg);
+    AssertFault(ff_util::INITIALIZATION_FAILED, msg, GetTimeNow());
     return;
   }
 

mobility/choreographer/src/choreographer_component.cc

@@ -125,7 +125,7 @@ class ChoreographerComponent : public ff_util::FreeFlyerComponent {
       [this](FSM::Event const& event) -> FSM::State {
         if (!Control(IDLE))
           AssertFault(ff_util::INITIALIZATION_FAILED,
-                      "Cannot call Idle() action");
+                      "Cannot call Idle() action", GetTimeNow());
         return STATE::IDLE;
       });
     // [1]
@@ -475,7 +475,7 @@ class ChoreographerComponent : public ff_util::FreeFlyerComponent {
     cfg_.AddFile("mobility/choreographer.config");
     if (!cfg_.Initialize(nh)) {
       AssertFault(ff_util::INITIALIZATION_FAILED,
-                  "Could not start config server");
+                  "Could not start config server", GetTimeNow());
       return;
     }
 
@@ -485,15 +485,15 @@ class ChoreographerComponent : public ff_util::FreeFlyerComponent {
     // Get the default flight mode, it will be published later
     if (!ff_util::FlightUtil::GetInitialFlightMode(flight_mode_)) {
       AssertFault(ff_util::INITIALIZATION_FAILED,
-                  "Problem with initial flight mode");
+                  "Problem with initial flight mode", GetTimeNow());
       return;
     }
 
     // Get the default inertia parameters, it will be published later
     geometry_msgs::InertiaStamped msg;
     if (!ff_util::FlightUtil::GetInertiaConfig(msg)) {
       AssertFault(ff_util::INITIALIZATION_FAILED,
-                  "Problem with default inertia");
+                  "Problem with default inertia", GetTimeNow());
       return;
     }
 

mobility/framestore/src/framestore.cc

@@ -120,7 +120,7 @@ class FrameStore : public ff_util::FreeFlyerComponent {
   // Deal with a fault in a responsible manner
   void InitFault(std::string const& msg ) {
     FF_ERROR_STREAM(msg);
-    AssertFault(ff_util::INITIALIZATION_FAILED, msg);
+    AssertFault(ff_util::INITIALIZATION_FAILED, msg, GetTimeNow());
     return;
   }
 

mobility/mapper/src/mapper_component.cc

@@ -40,7 +40,7 @@ void MapperComponent::Initialize(NodeHandle &nh) {
   cfg_.AddFile("mobility/mapper.config");
   if (!cfg_.Initialize(nh)) {
     AssertFault(ff_util::INITIALIZATION_FAILED,
-                "Could not start config server");
+                "Could not start config server", GetTimeNow());
     return;
   }
 

shared/ff_util/include/ff_util/ff_component.h

@@ -89,7 +89,7 @@ class FreeFlyerComponent {
 
   void AssertFault(FaultKeys enum_key,
                    std::string const& message,
-                   ros::Time time_fault_occurred = rclcpp::Clock().now());
+                   ros::Time time_fault_occurred);
   void ClearAllFaults();
   void ClearFault(FaultKeys enum_key);
   void PrintFaults();

simulation/src/gazebo_model_plugin_pmc/gazebo_model_plugin_pmc.cc

@@ -177,7 +177,7 @@ class GazeboModelPluginPmc : public FreeFlyerModelPlugin {
     if (!GetParams()) {
       FF_ERROR("PMC Actuator: Failed to get parameters.");
       AssertFault(ff_util::INITIALIZATION_FAILED,
-                  "Could not get PMC parameters");
+                  "Could not get PMC parameters", GetTimeNow());
     }
 
     // If we specify a frame name different to our sensor tag name