Update examples based on new profiles

tesseract-robotics · Jan 1, 2025 · 0232eea · 0232eea
1 parent 5d4a628
commit 0232eea
Show file tree

Hide file tree

Showing 9 changed files with 41 additions and 8 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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();
 }