From 7604d424630c4ed262f2d847a1d99eb38b3a9a0a Mon Sep 17 00:00:00 2001
From: Levi Armstrong <levi.armstrong@gmail.com>
Date: Wed, 1 Dec 2021 11:02:48 -0600
Subject: [PATCH] Add timing to basic cartesian and glass upright example

---
 tesseract_ros_examples/src/basic_cartesian_example.cpp |  8 ++++++++
 tesseract_ros_examples/src/glass_upright_example.cpp   | 10 ++++++++++
 2 files changed, 18 insertions(+)

diff --git a/tesseract_ros_examples/src/basic_cartesian_example.cpp b/tesseract_ros_examples/src/basic_cartesian_example.cpp
index 5bc552686..48c6d20b2 100644
--- a/tesseract_ros_examples/src/basic_cartesian_example.cpp
+++ b/tesseract_ros_examples/src/basic_cartesian_example.cpp
@@ -38,6 +38,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_environment/utils.h>
 #include <tesseract_rosutils/plotting.h>
 #include <tesseract_rosutils/utils.h>
+#include <tesseract_common/timer.h>
 #include <tesseract_command_language/command_language.h>
 #include <tesseract_command_language/utils/utils.h>
 #include <tesseract_process_managers/core/process_planning_server.h>
@@ -221,6 +222,9 @@ bool BasicCartesianExample::run()
     composite_profile->collision_cost_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
     composite_profile->collision_constraint_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
     composite_profile->smooth_velocities = true;
+    composite_profile->smooth_accelerations = false;
+    composite_profile->smooth_jerks = false;
+    composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
     planning_server.getProfiles()->addProfile<TrajOptIfoptCompositeProfile>(
         profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile);
 
@@ -265,8 +269,12 @@ bool BasicCartesianExample::run()
     plotter->waitForInput("Hit Enter to solve for trajectory.");
 
   // Solve process plan
+  tesseract_common::Timer stopwatch;
+  stopwatch.start();
   ProcessPlanningFuture response = planning_server.run(request);
   planning_server.waitForAll();
+  stopwatch.stop();
+  ROS_INFO("Planning took %f seconds.", stopwatch.elapsedSeconds());
 
   // Plot Process Trajectory
   if (rviz_ && plotter != nullptr && plotter->isConnected())
diff --git a/tesseract_ros_examples/src/glass_upright_example.cpp b/tesseract_ros_examples/src/glass_upright_example.cpp
index a03b215e6..1826acd05 100644
--- a/tesseract_ros_examples/src/glass_upright_example.cpp
+++ b/tesseract_ros_examples/src/glass_upright_example.cpp
@@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_environment/utils.h>
 #include <tesseract_rosutils/plotting.h>
 #include <tesseract_rosutils/utils.h>
+#include <tesseract_common/timer.h>
 #include <tesseract_command_language/command_language.h>
 #include <tesseract_command_language/utils/utils.h>
 #include <tesseract_planning_server/tesseract_planning_server.h>
@@ -200,6 +201,9 @@ bool GlassUprightExample::run()
     composite_profile->collision_constraint_config->collision_margin_data.setDefaultCollisionMargin(0.01);
     composite_profile->collision_constraint_config->collision_margin_buffer = 0.01;
     composite_profile->smooth_velocities = true;
+    composite_profile->smooth_accelerations = false;
+    composite_profile->smooth_jerks = false;
+    composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
     planning_server.getProfiles()->addProfile<TrajOptIfoptCompositeProfile>(
         profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile);
 
@@ -220,10 +224,12 @@ bool GlassUprightExample::run()
     composite_profile->collision_cost_config.enabled = true;
     composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::DISCRETE_CONTINUOUS;
     composite_profile->collision_cost_config.safety_margin = 0.01;
+    composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
     composite_profile->collision_cost_config.coeff = 1;
     composite_profile->collision_constraint_config.enabled = true;
     composite_profile->collision_constraint_config.type = trajopt::CollisionEvaluatorType::DISCRETE_CONTINUOUS;
     composite_profile->collision_constraint_config.safety_margin = 0.01;
+    composite_profile->collision_constraint_config.safety_margin_buffer = 0.01;
     composite_profile->collision_constraint_config.coeff = 1;
     composite_profile->smooth_velocities = true;
     composite_profile->smooth_accelerations = false;
@@ -253,8 +259,12 @@ bool GlassUprightExample::run()
     plotter->waitForInput("Hit Enter to solve for trajectory.");
 
   // Solve process plan
+  tesseract_common::Timer stopwatch;
+  stopwatch.start();
   ProcessPlanningFuture response = planning_server.run(request);
   planning_server.waitForAll();
+  stopwatch.stop();
+  ROS_INFO("Planning took %f seconds.", stopwatch.elapsedSeconds());
 
   // Plot Process Trajectory
   if (rviz_ && plotter != nullptr && plotter->isConnected())