From 0232eea8f624f2eeed47e08b938798dbd7c67cce Mon Sep 17 00:00:00 2001
From: Levi Armstrong <levi.armstrong@gmail.com>
Date: Wed, 1 Jan 2025 09:43:20 -0600
Subject: [PATCH] Update examples based on new profiles

---
 .../src/tesseract_planning_server.cpp                    | 9 +++++----
 tesseract_ros_examples/launch/car_seat_example.launch    | 4 ++++
 .../launch/pick_and_place_example.launch                 | 4 ++++
 .../launch/puzzle_piece_auxillary_axes_example.launch    | 4 ++++
 .../launch/puzzle_piece_example.launch                   | 4 ++++
 tesseract_ros_examples/src/car_seat_example_node.cpp     | 6 +++++-
 .../src/pick_and_place_example_node.cpp                  | 6 +++++-
 .../src/puzzle_piece_auxillary_axes_example_node.cpp     | 6 +++++-
 tesseract_ros_examples/src/puzzle_piece_example_node.cpp | 6 +++++-
 9 files changed, 41 insertions(+), 8 deletions(-)

diff --git a/tesseract_planning_server/src/tesseract_planning_server.cpp b/tesseract_planning_server/src/tesseract_planning_server.cpp
index fc8713cb6..7cac9f47d 100644
--- a/tesseract_planning_server/src/tesseract_planning_server.cpp
+++ b/tesseract_planning_server/src/tesseract_planning_server.cpp
@@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <console_bridge/console.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
+#include <tesseract_scene_graph/scene_state.h>
 #include <tesseract_planning_server/tesseract_planning_server.h>
 #include <tesseract_task_composer/core/task_composer_server.h>
 #include <tesseract_task_composer/core/task_composer_context.h>
@@ -49,7 +50,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_motion_planners/trajopt/profile/trajopt_profile.h>
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>
-#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
+#include <tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h>
 #endif
 
 #ifdef TESSERACT_PLANNING_SERVER_HAS_TRAJOPT_IFOPT
@@ -60,7 +61,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #ifdef TESSERACT_PLANNING_SERVER_HAS_OMPL
 #include <tesseract_motion_planners/ompl/profile/ompl_profile.h>
-#include <tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h>
+#include <tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h>
 #endif
 
 #include <tesseract_motion_planners/simple/profile/simple_planner_profile.h>
@@ -177,7 +178,7 @@ struct TesseractPlanningServer::Implementation
                          std::make_shared<tesseract_planning::TrajOptDefaultCompositeProfile>());
     profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE,
                          tesseract_planning::DEFAULT_PROFILE_KEY,
-                         std::make_shared<tesseract_planning::TrajOptDefaultSolverProfile>());
+                         std::make_shared<tesseract_planning::TrajOptOSQPSolverProfile>());
 #endif
 
     // Add TrajOpt IFOPT Default Profiles
@@ -201,7 +202,7 @@ struct TesseractPlanningServer::Implementation
 #ifdef TESSERACT_PLANNING_SERVER_HAS_OMPL
     profiles->addProfile(OMPL_DEFAULT_NAMESPACE,
                          tesseract_planning::DEFAULT_PROFILE_KEY,
-                         std::make_shared<tesseract_planning::OMPLDefaultPlanProfile>());
+                         std::make_shared<tesseract_planning::OMPLRealVectorPlanProfile>());
 #endif
   }
 
diff --git a/tesseract_ros_examples/launch/car_seat_example.launch b/tesseract_ros_examples/launch/car_seat_example.launch
index 6ff8a2524..621c9bde9 100644
--- a/tesseract_ros_examples/launch/car_seat_example.launch
+++ b/tesseract_ros_examples/launch/car_seat_example.launch
@@ -4,6 +4,8 @@
   <arg name="robot_description" default="robot_description"/>
   <arg name="plotting" default="true"/>
   <arg name="rviz" default="true"/>
+  <arg name="ifopt" default="false"/>
+  <arg name="debug" default="false"/>
   <arg name="testing" default="false"/>
 
   <!-- Load universal robot description format (URDF) -->
@@ -19,6 +21,8 @@
     <node pkg="tesseract_ros_examples" type="tesseract_ros_examples_car_seat_example_node" name="tesseract_ros_examples_car_seat_example_node" output="screen">
       <param name="plotting" type="bool" value="$(arg plotting)"/>
       <param name="rviz" type="bool" value="$(arg rviz)"/>
+      <param name="ifopt" type="bool" value="$(arg ifopt)"/>
+      <param name="debug" type="bool" value="$(arg debug)"/>
     </node>
 
     <!-- Launch visualization -->
diff --git a/tesseract_ros_examples/launch/pick_and_place_example.launch b/tesseract_ros_examples/launch/pick_and_place_example.launch
index 3fc79f161..5bf3cffea 100644
--- a/tesseract_ros_examples/launch/pick_and_place_example.launch
+++ b/tesseract_ros_examples/launch/pick_and_place_example.launch
@@ -7,6 +7,8 @@
     <arg name="box_x" default="0.15"/>
     <arg name="box_y" default="0.4"/>
     <arg name="rviz" default="true"/>
+    <arg name="ifopt" default="false"/>
+    <arg name="debug" default="false"/>
     <arg name="testing" default="false"/>
 
     <!-- Load Robot Model    -->
@@ -26,6 +28,8 @@
           <param name="plotting" value="$(arg plotting)"/>
           <param name="file_write_cb" value="$(arg file_write_cb)" />
           <param name="rviz" type="bool" value="$(arg rviz)"/>
+          <param name="ifopt" type="bool" value="$(arg ifopt)"/>
+          <param name="debug" type="bool" value="$(arg debug)"/>
       </node>
 
       <!-- Launch visualization -->
diff --git a/tesseract_ros_examples/launch/puzzle_piece_auxillary_axes_example.launch b/tesseract_ros_examples/launch/puzzle_piece_auxillary_axes_example.launch
index 032a257e7..b32b5ff25 100644
--- a/tesseract_ros_examples/launch/puzzle_piece_auxillary_axes_example.launch
+++ b/tesseract_ros_examples/launch/puzzle_piece_auxillary_axes_example.launch
@@ -5,6 +5,8 @@
   <arg name="robot_description" default="robot_description"/>
   <arg name="plotting" default="true"/>
   <arg name="rviz" default="true"/>
+  <arg name="ifopt" default="false"/>
+  <arg name="debug" default="false"/>
   <arg name="testing" default="false"/>
 
   <!-- Load universal robot description format (URDF) -->
@@ -20,6 +22,8 @@
     <node pkg="tesseract_ros_examples" type="tesseract_ros_examples_puzzle_piece_auxillary_axes_example_node" name="tesseract_ros_examples_puzzle_piece_auxillary_axes_example_node" output="screen">
       <param name="plotting" type="bool" value="$(arg plotting)"/>
       <param name="rviz" type="bool" value="$(arg rviz)"/>
+      <param name="ifopt" type="bool" value="$(arg ifopt)"/>
+      <param name="debug" type="bool" value="$(arg debug)"/>
     </node>
 
     <!-- Launch visualization -->
diff --git a/tesseract_ros_examples/launch/puzzle_piece_example.launch b/tesseract_ros_examples/launch/puzzle_piece_example.launch
index bbd4963e7..f211f65c2 100644
--- a/tesseract_ros_examples/launch/puzzle_piece_example.launch
+++ b/tesseract_ros_examples/launch/puzzle_piece_example.launch
@@ -5,6 +5,8 @@
   <arg name="robot_description" default="robot_description"/>
   <arg name="plotting" default="true"/>
   <arg name="rviz" default="true"/>
+  <arg name="ifopt" default="false"/>
+  <arg name="debug" default="false"/>
   <arg name="testing" default="false"/>
 
   <!-- Load universal robot description format (URDF) -->
@@ -20,6 +22,8 @@
     <node pkg="tesseract_ros_examples" type="tesseract_ros_examples_puzzle_piece_example_node" name="tesseract_ros_examples_puzzle_piece_example_node" output="screen">
       <param name="plotting" type="bool" value="$(arg plotting)"/>
       <param name="rviz" type="bool" value="$(arg rviz)"/>
+      <param name="ifopt" type="bool" value="$(arg ifopt)"/>
+      <param name="debug" type="bool" value="$(arg debug)"/>
     </node>
 
     <!-- Launch visualization -->
diff --git a/tesseract_ros_examples/src/car_seat_example_node.cpp b/tesseract_ros_examples/src/car_seat_example_node.cpp
index 2448c99e3..dafd34bf3 100644
--- a/tesseract_ros_examples/src/car_seat_example_node.cpp
+++ b/tesseract_ros_examples/src/car_seat_example_node.cpp
@@ -56,10 +56,14 @@ int main(int argc, char** argv)
 
   bool plotting = true;
   bool rviz = true;
+  bool ifopt = false;
+  bool debug = false;
 
   // Get ROS Parameters
   pnh.param("plotting", plotting, plotting);
   pnh.param("rviz", rviz, rviz);
+  pnh.param("ifopt", ifopt, ifopt);
+  pnh.param("debug", debug, debug);
 
   // Initial setup
   std::string urdf_xml_string, srdf_xml_string;
@@ -80,6 +84,6 @@ int main(int argc, char** argv)
   if (plotting)
     plotter = std::make_shared<ROSPlotting>(env->getSceneGraph()->getRoot());
 
-  CarSeatExample example(env, plotter);
+  CarSeatExample example(env, plotter, ifopt, debug);
   example.run();
 }
diff --git a/tesseract_ros_examples/src/pick_and_place_example_node.cpp b/tesseract_ros_examples/src/pick_and_place_example_node.cpp
index 57832f606..83f0b9809 100644
--- a/tesseract_ros_examples/src/pick_and_place_example_node.cpp
+++ b/tesseract_ros_examples/src/pick_and_place_example_node.cpp
@@ -56,10 +56,14 @@ int main(int argc, char** argv)
 
   bool plotting = true;
   bool rviz = true;
+  bool ifopt = false;
+  bool debug = false;
 
   // Get ROS Parameters
   pnh.param("plotting", plotting, plotting);
   pnh.param("rviz", rviz, rviz);
+  pnh.param("ifopt", ifopt, ifopt);
+  pnh.param("debug", debug, debug);
 
   // Initial setup
   std::string urdf_xml_string, srdf_xml_string;
@@ -80,6 +84,6 @@ int main(int argc, char** argv)
   if (plotting)
     plotter = std::make_shared<ROSPlotting>(env->getSceneGraph()->getRoot());
 
-  PickAndPlaceExample example(env, plotter);
+  PickAndPlaceExample example(env, plotter, ifopt, debug);
   example.run();
 }
diff --git a/tesseract_ros_examples/src/puzzle_piece_auxillary_axes_example_node.cpp b/tesseract_ros_examples/src/puzzle_piece_auxillary_axes_example_node.cpp
index 1ce085a78..a63c3ce93 100644
--- a/tesseract_ros_examples/src/puzzle_piece_auxillary_axes_example_node.cpp
+++ b/tesseract_ros_examples/src/puzzle_piece_auxillary_axes_example_node.cpp
@@ -56,10 +56,14 @@ int main(int argc, char** argv)
 
   bool plotting = true;
   bool rviz = true;
+  bool ifopt = false;
+  bool debug = false;
 
   // Get ROS Parameters
   pnh.param("plotting", plotting, plotting);
   pnh.param("rviz", rviz, rviz);
+  pnh.param("ifopt", ifopt, ifopt);
+  pnh.param("debug", debug, debug);
 
   // Initial setup
   std::string urdf_xml_string, srdf_xml_string;
@@ -80,6 +84,6 @@ int main(int argc, char** argv)
   if (plotting)
     plotter = std::make_shared<ROSPlotting>(env->getSceneGraph()->getRoot());
 
-  PuzzlePieceAuxillaryAxesExample example(env, plotter);
+  PuzzlePieceAuxillaryAxesExample example(env, plotter, ifopt, debug);
   example.run();
 }
diff --git a/tesseract_ros_examples/src/puzzle_piece_example_node.cpp b/tesseract_ros_examples/src/puzzle_piece_example_node.cpp
index cc05b1f97..138883e74 100644
--- a/tesseract_ros_examples/src/puzzle_piece_example_node.cpp
+++ b/tesseract_ros_examples/src/puzzle_piece_example_node.cpp
@@ -56,10 +56,14 @@ int main(int argc, char** argv)
 
   bool plotting = true;
   bool rviz = true;
+  bool ifopt = false;
+  bool debug = false;
 
   // Get ROS Parameters
   pnh.param("plotting", plotting, plotting);
   pnh.param("rviz", rviz, rviz);
+  pnh.param("ifopt", ifopt, ifopt);
+  pnh.param("debug", debug, debug);
 
   // Initial setup
   std::string urdf_xml_string, srdf_xml_string;
@@ -80,6 +84,6 @@ int main(int argc, char** argv)
   if (plotting)
     plotter = std::make_shared<ROSPlotting>(env->getSceneGraph()->getRoot());
 
-  PuzzlePieceExample example(env, plotter);
+  PuzzlePieceExample example(env, plotter, ifopt, debug);
   example.run();
 }