More clock changes

Signed-off-by: methylDragon <methylDragon@gmail.com>
locusrobotics · Nov 11, 2022 · f22d1d4 · f22d1d4
1 parent c18049b
commit f22d1d4
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 11 deletions.
diff --git a/fuse_graphs/include/fuse_graphs/hash_graph.h b/fuse_graphs/include/fuse_graphs/hash_graph.h
@@ -330,7 +330,8 @@ class HashGraph : public fuse_core::Graph
    */
   ceres::Solver::Summary optimizeFor(
     const rclcpp::Duration& max_optimization_time,
-    const ceres::Solver::Options& options = ceres::Solver::Options()) override;
+    const ceres::Solver::Options& options = ceres::Solver::Options(),
+    const rclcpp::Clock& clock = rclcpp::Clock::Clock(RCL_STEADY_TIME)) override;
 
   /**
    * @brief Evalute the values of the current set of variables, given the current set of constraints.

diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp
@@ -428,9 +428,9 @@ ceres::Solver::Summary HashGraph::optimize(const ceres::Solver::Options& options
 
 ceres::Solver::Summary HashGraph::optimizeFor(
   const rclcpp::Duration& max_optimization_time,
-  const ceres::Solver::Options& options)
+  const ceres::Solver::Options& options,
+  const rclcpp::Clock& clock)
 {
-  auto clock = rclcpp::Clock::Clock(RCL_STEADY_TIME);
   auto start = clock.now();
 
   // Construct the ceres::Problem object from scratch

diff --git a/fuse_tutorials/src/range_sensor_simulator.cpp b/fuse_tutorials/src/range_sensor_simulator.cpp
@@ -194,7 +194,7 @@ nav_msgs::Odometry::ConstPtr robotToOdometry(const Robot& state)
  * The state estimator will not run until it has been sent a starting pose.
  */
 void initializeStateEstimation(
-  const Robot& state, rclcpp::Clock::SharedPtr clock=nullptr)
+  const Robot& state, const rclcpp::Clock& clock)
 {
   // Send the initial localization signal to the state estimator
   auto srv = fuse_models::SetPose();
@@ -217,12 +217,7 @@ void initializeStateEstimation(
   while (!success)
   {
     rclcpp::Duration::from_seconds(0.1).sleep();
-
-    if (clock != nullptr) {
-      srv.request.pose.header.stamp = clock->now();
-    } else {
-      srv.request.pose.header.stamp = rclcpp::Clock(RCL_SYSTEM_TIME).now();
-    }
+    srv.request.pose.header.stamp = clock->now();
     ros::service::call("/state_estimation/set_pose", srv);
     success = srv.response.success;
   }
@@ -377,7 +372,7 @@ int main(int argc, char **argv)
   state.vyaw = state.vx / ROBOT_PATH_RADIUS;
 
   // Send the initial localization pose to the state estimator
-  initializeStateEstimation(state);
+  initializeStateEstimation(state, *node->get_clock());
 
   // Simulate the robot traveling in a circular path
   auto rate = rclcpp::Rate(10.0);