From 86b5f258812307f7a267dc1e9a58fd7de9809c0a Mon Sep 17 00:00:00 2001
From: Roelof Oomen <rj.oomen@isohorti.com>
Date: Mon, 15 Jan 2024 08:47:38 +0100
Subject: [PATCH 1/2] Adding Trajopt_Ifopt option to all examples

---
 tesseract_ros_examples/launch/car_seat_example.launch    | 4 ++++
 .../launch/freespace_hybrid_example.launch               | 4 ++++
 .../launch/pick_and_place_example.launch                 | 6 +++++-
 .../launch/puzzle_piece_auxillary_axes_example.launch    | 4 ++++
 .../launch/puzzle_piece_example.launch                   | 4 ++++
 tesseract_ros_examples/src/car_seat_example_node.cpp     | 9 ++++++++-
 .../src/freespace_hybrid_example_node.cpp                | 9 ++++++++-
 .../src/pick_and_place_example_node.cpp                  | 9 ++++++++-
 .../src/puzzle_piece_auxillary_axes_example_node.cpp     | 9 ++++++++-
 tesseract_ros_examples/src/puzzle_piece_example_node.cpp | 9 ++++++++-
 10 files changed, 61 insertions(+), 6 deletions(-)

diff --git a/tesseract_ros_examples/launch/car_seat_example.launch b/tesseract_ros_examples/launch/car_seat_example.launch
index 55a3fc92..2f732e6b 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" exec="tesseract_ros_examples_car_seat_example_node" name="tesseract_ros_examples_car_seat_example_node" output="screen">
       <param name="plotting" type="bool" value="$(var plotting)"/>
       <param name="rviz" type="bool" value="$(var rviz)"/>
+      <param name="ifopt" type="bool" value="$(var ifopt)"/>
+      <param name="debug" type="bool" value="$(var debug)"/>
       <param name="$(var robot_description)" value="$(var robot_description_file)" type="str"/>
       <param name="$(var robot_description)_semantic" value="$(var robot_description_semantic_file)" type="str"/>
     </node>
diff --git a/tesseract_ros_examples/launch/freespace_hybrid_example.launch b/tesseract_ros_examples/launch/freespace_hybrid_example.launch
index 3d8df9bb..9011b8de 100644
--- a/tesseract_ros_examples/launch/freespace_hybrid_example.launch
+++ b/tesseract_ros_examples/launch/freespace_hybrid_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"/>
   <arg name="range" default="0.01"/>
   <arg name="planning_time" default="10.0"/>
@@ -22,6 +24,8 @@
     <node pkg="tesseract_ros_examples" exec="tesseract_ros_examples_freespace_hybrid_example_node" output="screen">
       <param name="plotting" type="bool" value="$(var plotting)"/>
       <param name="rviz" type="bool" value="$(var rviz)"/>
+      <param name="ifopt" type="bool" value="$(var ifopt)"/>
+      <param name="debug" type="bool" value="$(var debug)"/>
       <param name="range" type="float" value="$(var range)"/>
       <param name="planning_time" type="float" value="$(var planning_time)"/>
       <param name="$(var robot_description)" value="$(var robot_description_file)" type="str"/>
diff --git a/tesseract_ros_examples/launch/pick_and_place_example.launch b/tesseract_ros_examples/launch/pick_and_place_example.launch
index 1cd6ced1..85cabc01 100644
--- a/tesseract_ros_examples/launch/pick_and_place_example.launch
+++ b/tesseract_ros_examples/launch/pick_and_place_example.launch
@@ -8,6 +8,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    -->
@@ -27,8 +29,10 @@
         <param name="plotting" type="bool" value="$(var plotting)"/>
         <param name="file_write_cb" value="$(var file_write_cb)" />
         <param name="rviz" type="bool" value="$(var rviz)"/>
+        <param name="ifopt" type="bool" value="$(var ifopt)"/>
+        <param name="debug" type="bool" value="$(var debug)"/>
         <param name="$(var robot_description)" value="$(var robot_description_file)" type="str"/>
-      <param name="$(var robot_description)_semantic" value="$(var robot_description_semantic_file)" type="str"/>
+        <param name="$(var robot_description)_semantic" value="$(var robot_description_semantic_file)" type="str"/>
       </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 9a3f89ca..74385419 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" exec="tesseract_ros_examples_puzzle_piece_auxillary_axes_example_node" output="screen">
       <param name="plotting" type="bool" value="$(var plotting)"/>
       <param name="rviz" type="bool" value="$(var rviz)"/>
+      <param name="ifopt" type="bool" value="$(var ifopt)"/>
+      <param name="debug" type="bool" value="$(var debug)"/>
       <param name="$(var robot_description)" value="$(var robot_description_file)" type="str"/>
       <param name="$(var robot_description)_semantic" value="$(var robot_description_semantic_file)" type="str"/>
     </node>
diff --git a/tesseract_ros_examples/launch/puzzle_piece_example.launch b/tesseract_ros_examples/launch/puzzle_piece_example.launch
index 2cd73899..1a049567 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" exec="tesseract_ros_examples_puzzle_piece_example_node" output="screen">
       <param name="plotting" type="bool" value="$(var plotting)"/>
       <param name="rviz" type="bool" value="$(var rviz)"/>
+      <param name="ifopt" type="bool" value="$(var ifopt)"/>
+      <param name="debug" type="bool" value="$(var debug)"/>
       <param name="$(var robot_description)" value="$(var robot_description_file)" type="str"/>
       <param name="$(var robot_description)_semantic" value="$(var robot_description_semantic_file)" type="str"/>
     </node>
diff --git a/tesseract_ros_examples/src/car_seat_example_node.cpp b/tesseract_ros_examples/src/car_seat_example_node.cpp
index a87ed744..8a9acacc 100644
--- a/tesseract_ros_examples/src/car_seat_example_node.cpp
+++ b/tesseract_ros_examples/src/car_seat_example_node.cpp
@@ -51,11 +51,18 @@ int main(int argc, char** argv)
   // Get ROS Parameters
   bool plotting = node->declare_parameter("plotting", true);
   bool rviz = node->declare_parameter("rviz", true);
+  bool ifopt = node->declare_parameter("ifopt", false);
+  bool debug = node->declare_parameter("debug", false);
 
   // Initial setup
   std::string urdf_xml_string = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
   std::string srdf_xml_string = node->declare_parameter(ROBOT_SEMANTIC_PARAM, "");
 
+  if (ifopt)
+  {
+    RCLCPP_INFO(node->get_logger(), "Using TrajOpt Ifopt!");
+  }
+
   auto env = std::make_shared<tesseract_environment::Environment>();
   auto locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();
   if (!env->init(urdf_xml_string, srdf_xml_string, locator))
@@ -72,7 +79,7 @@ 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);
   rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(5.0)));
 
   example.run();
diff --git a/tesseract_ros_examples/src/freespace_hybrid_example_node.cpp b/tesseract_ros_examples/src/freespace_hybrid_example_node.cpp
index 45fea4ba..c8617f08 100644
--- a/tesseract_ros_examples/src/freespace_hybrid_example_node.cpp
+++ b/tesseract_ros_examples/src/freespace_hybrid_example_node.cpp
@@ -51,6 +51,8 @@ int main(int argc, char** argv)
   // Get ROS Parameters
   bool plotting = node->declare_parameter("plotting", true);
   bool rviz = node->declare_parameter("rviz", true);
+  bool ifopt = node->declare_parameter("ifopt", false);
+  bool debug = node->declare_parameter("debug", false);
   double range = node->declare_parameter("range", 0.01);
   double planning_time = node->declare_parameter("planning_time", 60.0);
 
@@ -58,6 +60,11 @@ int main(int argc, char** argv)
   std::string urdf_xml_string = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
   std::string srdf_xml_string = node->declare_parameter(ROBOT_SEMANTIC_PARAM, "");
 
+  if (ifopt)
+  {
+    RCLCPP_INFO(node->get_logger(), "Using TrajOpt Ifopt!");
+  }
+
   auto env = std::make_shared<tesseract_environment::Environment>();
   auto locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();
   if (!env->init(urdf_xml_string, srdf_xml_string, locator))
@@ -74,7 +81,7 @@ int main(int argc, char** argv)
   if (plotting)
     plotter = std::make_shared<ROSPlotting>(env->getSceneGraph()->getRoot());
 
-  FreespaceHybridExample example(env, plotter, range, planning_time);
+  FreespaceHybridExample example(env, plotter, ifopt, debug, range, planning_time);
   rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(5.0)));
 
   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 496a8b53..117b240f 100644
--- a/tesseract_ros_examples/src/pick_and_place_example_node.cpp
+++ b/tesseract_ros_examples/src/pick_and_place_example_node.cpp
@@ -51,10 +51,17 @@ int main(int argc, char** argv)
   // Get ROS Parameters
   bool plotting = node->declare_parameter("plotting", true);
   bool rviz = node->declare_parameter("rviz", true);
+  bool ifopt = node->declare_parameter("ifopt", false);
+  bool debug = node->declare_parameter("debug", false);
 
   // Initial setup
   std::string urdf_xml_string = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
   std::string srdf_xml_string = node->declare_parameter(ROBOT_SEMANTIC_PARAM, "");
+  
+  if (ifopt)
+  {
+    RCLCPP_INFO(node->get_logger(), "Using TrajOpt Ifopt!");
+  }
 
   auto env = std::make_shared<tesseract_environment::Environment>();
   auto locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();
@@ -72,7 +79,7 @@ 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);
   rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(5.0)));
 
   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 8a2b6d85..bac1e373 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
@@ -51,11 +51,18 @@ int main(int argc, char** argv)
   // Get ROS Parameters
   bool plotting = node->declare_parameter("plotting", true);
   bool rviz = node->declare_parameter("rviz", true);
+  bool ifopt = node->declare_parameter("ifopt", false);
+  bool debug = node->declare_parameter("debug", false);
 
   // Initial setup
   std::string urdf_xml_string = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
   std::string srdf_xml_string = node->declare_parameter(ROBOT_SEMANTIC_PARAM, "");
 
+  if (ifopt)
+  {
+    RCLCPP_INFO(node->get_logger(), "Using TrajOpt Ifopt!");
+  }
+
   auto env = std::make_shared<tesseract_environment::Environment>();
   auto locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();
   if (!env->init(urdf_xml_string, srdf_xml_string, locator))
@@ -72,7 +79,7 @@ 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);
   rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(5.0)));
 
   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 4d777267..175defaa 100644
--- a/tesseract_ros_examples/src/puzzle_piece_example_node.cpp
+++ b/tesseract_ros_examples/src/puzzle_piece_example_node.cpp
@@ -51,11 +51,18 @@ int main(int argc, char** argv)
   // Get ROS Parameters
   bool plotting = node->declare_parameter("plotting", true);
   bool rviz = node->declare_parameter("rviz", true);
+  bool ifopt = node->declare_parameter("ifopt", false);
+  bool debug = node->declare_parameter("debug", false);
 
   // Initial setup
   std::string urdf_xml_string = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
   std::string srdf_xml_string = node->declare_parameter(ROBOT_SEMANTIC_PARAM, "");
 
+  if (ifopt)
+  {
+    RCLCPP_INFO(node->get_logger(), "Using TrajOpt Ifopt!");
+  }
+
   auto env = std::make_shared<tesseract_environment::Environment>();
   auto locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();
   if (!env->init(urdf_xml_string, srdf_xml_string, locator))
@@ -72,7 +79,7 @@ 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);
   rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(5.0)));
 
   example.run();

From e471492cd573270e4f7707ec3072106a87909b98 Mon Sep 17 00:00:00 2001
From: Roelof Oomen <rj.oomen@isohorti.com>
Date: Mon, 15 Jan 2024 08:53:19 +0100
Subject: [PATCH 2/2] clang-format

---
 tesseract_ros_examples/src/pick_and_place_example_node.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 117b240f..9aed4ed0 100644
--- a/tesseract_ros_examples/src/pick_and_place_example_node.cpp
+++ b/tesseract_ros_examples/src/pick_and_place_example_node.cpp
@@ -57,7 +57,7 @@ int main(int argc, char** argv)
   // Initial setup
   std::string urdf_xml_string = node->declare_parameter(ROBOT_DESCRIPTION_PARAM, "");
   std::string srdf_xml_string = node->declare_parameter(ROBOT_SEMANTIC_PARAM, "");
-  
+
   if (ifopt)
   {
     RCLCPP_INFO(node->get_logger(), "Using TrajOpt Ifopt!");