Add timing to basic cartesian and glass upright example

tesseract-robotics · Dec 4, 2021 · 7604d42 · 7604d42
1 parent dc6865e
commit 7604d42
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 0 deletions.
diff --git 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
@@ -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())