From d20ef434807e7ef7f691b882896620230840cbe5 Mon Sep 17 00:00:00 2001
From: joris997 <joris.verhagen@gmail.com>
Date: Tue, 11 Apr 2023 14:12:37 +0200
Subject: [PATCH 1/9] planner_qp/traj_opt_basic initially done

---
 mobility/planner_qp/CMakeLists.txt            | 17 ++++----
 mobility/planner_qp/COLCON_IGNORE             |  0
 .../planner_qp/traj_opt_basic/CMakeLists.txt  | 42 +++++++++++--------
 .../include/traj_opt_basic/polynomial_basis.h |  2 +-
 .../planner_qp/traj_opt_basic/package.xml     | 10 +++--
 5 files changed, 40 insertions(+), 31 deletions(-)
 delete mode 100644 mobility/planner_qp/COLCON_IGNORE

diff --git a/mobility/planner_qp/CMakeLists.txt b/mobility/planner_qp/CMakeLists.txt
index 4a9a35a631..396f18d620 100644
--- a/mobility/planner_qp/CMakeLists.txt
+++ b/mobility/planner_qp/CMakeLists.txt
@@ -14,16 +14,17 @@
 # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 # License for the specific language governing permissions and limitations
 # under the License.
-
+cmake_minimum_required(VERSION 3.5)
+project(planner_qp)
 # this flag is needed by CGAL
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -frounding-math")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -frounding-math")
-set(NASA ON CACHE BOOL "Use NASA's wonderful build system on all traj_opt packages" FORCE)
+# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -frounding-math")
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -frounding-math")
+# set(NASA ON CACHE BOOL "Use NASA's wonderful build system on all traj_opt packages" FORCE)
 
 # QP planner
 add_subdirectory(traj_opt_basic)
-add_subdirectory(traj_opt_pro)
-add_subdirectory(traj_opt_msgs)
-add_subdirectory(traj_opt_ros)
+# add_subdirectory(traj_opt_pro)
+# add_subdirectory(traj_opt_msgs)
+# add_subdirectory(traj_opt_ros)
 
-add_subdirectory(planner_qp)
+# add_subdirectory(planner_qp)
diff --git a/mobility/planner_qp/COLCON_IGNORE b/mobility/planner_qp/COLCON_IGNORE
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/mobility/planner_qp/traj_opt_basic/CMakeLists.txt b/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
index c0092c14c9..38ea92a586 100644
--- a/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
@@ -15,22 +15,22 @@
 # License for the specific language governing permissions and limitations
 # under the License.
 
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 3.5)
 project(traj_opt_basic)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wno-deprecated-declarations  -Werror=return-type")
 
-find_package(catkin REQUIRED COMPONENTS
-)
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations  -Werror=return-type" )
+set( CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -Wall -O3 -fPIC" )
+
+find_package(ament_cmake REQUIRED COMPONENTS)
 
+find_package(rclcpp REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(Boost REQUIRED)
 
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES
-    traj_opt_basic
-)
-
 ###########
 ## Build ##
 ###########
@@ -42,27 +42,33 @@ include_directories(
   ${Boost_INCLUDE_DIRS}
 )
 
-add_library(traj_opt_basic
+add_library(${PROJECT_NAME} SHARED
 	src/msg_traj.cpp
 	src/polynomial_basis.cpp
 	src/trajectory.cpp)
-add_dependencies(traj_opt_basic ${catkin_EXPORTED_TARGETS})
-target_link_libraries(traj_opt_basic ${catkin_LIBRARIES})
+# target_link_libraries(traj_opt_basic ${catkin_LIBRARIES})
+target_compile_definitions(${PROJECT_NAME}
+  PRIVATE "COMPOSITION_BUILDING_DLL")
+target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES})
+ament_target_dependencies(${PROJECT_NAME} rclcpp)
+ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
 
 #############
 ## Install ##
 #############
 
 # Mark libraries for installation
-install(TARGETS ${PROJECT_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+install(TARGETS ${PROJECT_NAME} 
+  EXPORT traj_opt_basic
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
 )
 
 # Mark cpp header files for installation
 install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  DESTINATION include
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE
 )
\ No newline at end of file
diff --git a/mobility/planner_qp/traj_opt_basic/include/traj_opt_basic/polynomial_basis.h b/mobility/planner_qp/traj_opt_basic/include/traj_opt_basic/polynomial_basis.h
index 09e3cb2c71..6e8f05e0f9 100644
--- a/mobility/planner_qp/traj_opt_basic/include/traj_opt_basic/polynomial_basis.h
+++ b/mobility/planner_qp/traj_opt_basic/include/traj_opt_basic/polynomial_basis.h
@@ -42,7 +42,7 @@ class Basis {
 
   // evaluates ith element of basis at x where x is normalized to 0 to 1
   virtual decimal_t evaluate(decimal_t x, uint i) const = 0;
-  // innerproduct bewtween ith and jth basis functions
+  // innerproduct between ith and jth basis functions
   virtual decimal_t innerproduct(uint i, uint j) const = 0;
 
   virtual uint dim();
diff --git a/mobility/planner_qp/traj_opt_basic/package.xml b/mobility/planner_qp/traj_opt_basic/package.xml
index 3a2897793e..c90b7f51a7 100644
--- a/mobility/planner_qp/traj_opt_basic/package.xml
+++ b/mobility/planner_qp/traj_opt_basic/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>traj_opt_basic</name>
-  <version>0.0.1</version>
+  <version>0.0.0</version>
   <description>
   	The traj_opt_basic package
   </description>
@@ -14,7 +14,9 @@
   <maintainer email="astrobee-fsw@nx.arc.nasa.gov">
     Astrobee Flight Software
   </maintainer>
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>catkin_simple</build_depend>
+
+  <build_depend>ament_cmake</build_depend>
+  <build_depend>rclcpp</build_depend>
+
   <depend>eigen3</depend>
 </package>

From 12d319458d7e04d084f13c5aaa5229da1613917c Mon Sep 17 00:00:00 2001
From: joris997 <joris.verhagen@gmail.com>
Date: Sat, 15 Apr 2023 11:30:44 +0000
Subject: [PATCH 2/9] added traj_opt_msgs, now working on traj_opt_pro yet
 getting CMake import errors

---
 mobility/planner_qp/CMakeLists.txt            |  6 +--
 .../planner_qp/traj_opt_basic/CMakeLists.txt  | 14 +++---
 .../planner_qp/traj_opt_msgs/CMakeLists.txt   | 45 ++++++++++++-------
 .../traj_opt_msgs/msg/Trajectory.msg          |  2 +-
 mobility/planner_qp/traj_opt_msgs/package.xml | 25 ++++++++---
 .../planner_qp/traj_opt_pro/CMakeLists.txt    | 37 +++++++++------
 6 files changed, 84 insertions(+), 45 deletions(-)

diff --git a/mobility/planner_qp/CMakeLists.txt b/mobility/planner_qp/CMakeLists.txt
index 396f18d620..948ef1cb26 100644
--- a/mobility/planner_qp/CMakeLists.txt
+++ b/mobility/planner_qp/CMakeLists.txt
@@ -15,7 +15,7 @@
 # License for the specific language governing permissions and limitations
 # under the License.
 cmake_minimum_required(VERSION 3.5)
-project(planner_qp)
+project(planner_qp_main)
 # this flag is needed by CGAL
 # set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -frounding-math")
 # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -frounding-math")
@@ -23,8 +23,8 @@ project(planner_qp)
 
 # QP planner
 add_subdirectory(traj_opt_basic)
-# add_subdirectory(traj_opt_pro)
-# add_subdirectory(traj_opt_msgs)
+add_subdirectory(traj_opt_pro)
+add_subdirectory(traj_opt_msgs)
 # add_subdirectory(traj_opt_ros)
 
 # add_subdirectory(planner_qp)
diff --git a/mobility/planner_qp/traj_opt_basic/CMakeLists.txt b/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
index 38ea92a586..0408cb1271 100644
--- a/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
@@ -22,10 +22,9 @@ project(traj_opt_basic)
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
-set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations  -Werror=return-type" )
-set( CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -Wall -O3 -fPIC" )
+set( CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -Wall -O3 -fPIC -Wno-deprecated-declarations  -Werror=return-type" )
 
-find_package(ament_cmake REQUIRED COMPONENTS)
+find_package(ament_cmake REQUIRED)
 
 find_package(rclcpp REQUIRED)
 find_package(Eigen3 REQUIRED)
@@ -34,6 +33,7 @@ find_package(Boost REQUIRED)
 ###########
 ## Build ##
 ###########
+
 # Specify additional locations of header files
 include_directories(
   include
@@ -57,9 +57,11 @@ ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
 ## Install ##
 #############
 
+ament_export_include_directories(include)
+
 # Mark libraries for installation
 install(TARGETS ${PROJECT_NAME} 
-  EXPORT traj_opt_basic
+  EXPORT ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -71,4 +73,6 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION include
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE
-)
\ No newline at end of file
+)
+
+ament_package()
\ No newline at end of file
diff --git a/mobility/planner_qp/traj_opt_msgs/CMakeLists.txt b/mobility/planner_qp/traj_opt_msgs/CMakeLists.txt
index f192b5bd51..417a5a4161 100644
--- a/mobility/planner_qp/traj_opt_msgs/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_msgs/CMakeLists.txt
@@ -15,29 +15,40 @@
 # License for the specific language governing permissions and limitations
 # under the License.
 
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 3.5)
 
 project(traj_opt_msgs)
 
-find_package(catkin REQUIRED COMPONENTS
-  message_generation
-  std_msgs
-)
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+endif()
 
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
 # Generate messages in the 'msg' folder
-add_message_files(
-  FILES
-  Polynomial.msg
-  SolverInfo.msg
-  Spline.msg
-  Trajectory.msg
-)
+file(GLOB MSG_FILES msg/*.msg)
+foreach(SRC ${MSG_FILES})
+  get_filename_component(NAME ${SRC} NAME)
+  list(APPEND MSG_FILE_NAMES "msg/${NAME}")
+endforeach()
 
-generate_messages(
-  DEPENDENCIES std_msgs
-)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  ${MSG_FILE_NAMES}
 
-catkin_package(
-  LIBRARIES
+  DEPENDENCIES
+  builtin_interfaces
+  std_msgs
+
+  ADD_LINTER_TESTS
 )
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()
diff --git a/mobility/planner_qp/traj_opt_msgs/msg/Trajectory.msg b/mobility/planner_qp/traj_opt_msgs/msg/Trajectory.msg
index 9d8f0d15c5..ba3349c898 100644
--- a/mobility/planner_qp/traj_opt_msgs/msg/Trajectory.msg
+++ b/mobility/planner_qp/traj_opt_msgs/msg/Trajectory.msg
@@ -17,7 +17,7 @@
 #
 # Trajectory message used by the QP planner.
 
-Header header
+std_msgs/Header header
 int32 dimensions
 string[] dimension_names
 traj_opt_msgs/Spline[] data
\ No newline at end of file
diff --git a/mobility/planner_qp/traj_opt_msgs/package.xml b/mobility/planner_qp/traj_opt_msgs/package.xml
index 30ed6e9316..87ea3ac432 100644
--- a/mobility/planner_qp/traj_opt_msgs/package.xml
+++ b/mobility/planner_qp/traj_opt_msgs/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>traj_opt_msgs</name>
-  <version>0.0.1</version>
+  <version>0.0.0</version>
   <description>
   	The traj_opt_msgs package
   </description>
@@ -14,8 +14,21 @@
   <maintainer email="astrobee-fsw@nx.arc.nasa.gov">
     Astrobee Flight Software
   </maintainer>
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>catkin_simple</build_depend>
-  <depend>message_generation</depend>
-  <depend>std_msgs</depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <exec_depend>rosidl_default_generators</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
diff --git a/mobility/planner_qp/traj_opt_pro/CMakeLists.txt b/mobility/planner_qp/traj_opt_pro/CMakeLists.txt
index 9962de707f..16c9f44d4a 100644
--- a/mobility/planner_qp/traj_opt_pro/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_pro/CMakeLists.txt
@@ -16,8 +16,12 @@
 # under the License.
 
 cmake_minimum_required(VERSION 3.0)
-
 project(traj_opt_pro)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wno-deprecated-declarations  -Werror=return-type")
 
 ############################ Optional Build Params #############################
@@ -39,9 +43,11 @@ if(INTEL_MKL)
 endif()
 
 ## Find catkin macros and libraries
-find_package(catkin2 REQUIRED COMPONENTS
-  traj_opt_basic
-)
+find_package(ament_cmake REQUIRED)
+find_package(traj_opt_basic REQUIRED)
+# find_package(catkin2 REQUIRED COMPONENTS
+#   traj_opt_basic
+# )
 
 find_package(Eigen3 REQUIRED)
 
@@ -49,11 +55,11 @@ find_package(Eigen3 REQUIRED)
 LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../../cmake")
 find_package(OpenCV331 REQUIRED)
 
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES
-    traj_opt_pro fancy_custom_backend ${OpenCV_LIBRARIES}
-)
+# catkin_package(
+#   INCLUDE_DIRS include
+#   LIBRARIES
+#     traj_opt_pro fancy_custom_backend ${OpenCV_LIBRARIES}
+# )
 
 
 ###########
@@ -68,7 +74,7 @@ include_directories(
 )
 
 # Add libraries
-add_library(traj_opt_pro
+add_library(${PROJECT_NAME} SHARED
   src/polynomial_basis.cpp
   src/trajectory_solver.cpp)
 
@@ -76,14 +82,17 @@ if(BUILD_GUROBI)
   add_library(gurobi_backend
     src/gurobi_solver.cpp
     src/gurobi_trajectory.cpp )
-  target_link_libraries(gurobi_backend traj_opt_pro ${GUROBI_LIBRARIES})
+  target_link_libraries(gurobi_backend ${PROJECT_NAME} ${GUROBI_LIBRARIES})
 endif()
 
 add_library(fancy_custom_backend
   src/nonlinear_polynomial.cpp
   src/nonlinear_solver.cpp
   src/nonlinear_trajectory.cpp)
-target_link_libraries(fancy_custom_backend traj_opt_pro ${OpenCV_LIBRARIES})
+target_compile_definitions(${PROJECT_NAME}
+  PRIVATE "COMPOSITION_BUILDING_DLL")
+target_link_libraries(fancy_custom_backend ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+ament_target_dependencies(${PROJECT_NAME} traj_opt_basic)
 
 #############
 ## Install ##
@@ -101,4 +110,6 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE
-)
\ No newline at end of file
+)
+
+ament_package()
\ No newline at end of file

From a21e93322161c267a206716b61cbdbf538a6ec1d Mon Sep 17 00:00:00 2001
From: joris997 <joris.verhagen@gmail.com>
Date: Sun, 16 Apr 2023 09:01:10 +0000
Subject: [PATCH 3/9] fixed traj_opt_pro importing traj_opt_basic

---
 .../{CMakeLists.txt => _CMakeLists.txt}           |  4 +++-
 mobility/planner_qp/planner_qp/COLCON_IGNORE      |  0
 mobility/planner_qp/traj_opt_basic/CMakeLists.txt | 15 ++++++++++-----
 mobility/planner_qp/traj_opt_basic/package.xml    |  8 +++++++-
 mobility/planner_qp/traj_opt_pro/CMakeLists.txt   | 11 +++++++----
 mobility/planner_qp/traj_opt_ros/COLCON_IGNORE    |  0
 6 files changed, 27 insertions(+), 11 deletions(-)
 rename mobility/planner_qp/{CMakeLists.txt => _CMakeLists.txt} (95%)
 create mode 100644 mobility/planner_qp/planner_qp/COLCON_IGNORE
 create mode 100644 mobility/planner_qp/traj_opt_ros/COLCON_IGNORE

diff --git a/mobility/planner_qp/CMakeLists.txt b/mobility/planner_qp/_CMakeLists.txt
similarity index 95%
rename from mobility/planner_qp/CMakeLists.txt
rename to mobility/planner_qp/_CMakeLists.txt
index 948ef1cb26..080af3bc3d 100644
--- a/mobility/planner_qp/CMakeLists.txt
+++ b/mobility/planner_qp/_CMakeLists.txt
@@ -23,8 +23,10 @@ project(planner_qp_main)
 
 # QP planner
 add_subdirectory(traj_opt_basic)
-add_subdirectory(traj_opt_pro)
+# add_subdirectory(traj_opt_pro)
 add_subdirectory(traj_opt_msgs)
 # add_subdirectory(traj_opt_ros)
 
 # add_subdirectory(planner_qp)
+
+ament_package()
diff --git a/mobility/planner_qp/planner_qp/COLCON_IGNORE b/mobility/planner_qp/planner_qp/COLCON_IGNORE
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/mobility/planner_qp/traj_opt_basic/CMakeLists.txt b/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
index 0408cb1271..682e205610 100644
--- a/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
@@ -40,23 +40,28 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
   ${Boost_INCLUDE_DIRS}
+  ${traj_opt_basic_INCLUDE_DIRS}
 )
 
 add_library(${PROJECT_NAME} SHARED
 	src/msg_traj.cpp
 	src/polynomial_basis.cpp
-	src/trajectory.cpp)
-# target_link_libraries(traj_opt_basic ${catkin_LIBRARIES})
+	src/trajectory.cpp
+)
+# target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES})
 target_compile_definitions(${PROJECT_NAME}
   PRIVATE "COMPOSITION_BUILDING_DLL")
-target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES})
 ament_target_dependencies(${PROJECT_NAME} rclcpp)
-ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
+target_link_libraries(${PROJECT_NAME} ${traj_opt_basic_LIBRARIES})
+# ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
 
 #############
 ## Install ##
 #############
 
+ament_export_dependencies(
+)
+
 ament_export_include_directories(include)
 
 # Mark libraries for installation
@@ -69,7 +74,7 @@ install(TARGETS ${PROJECT_NAME}
 )
 
 # Mark cpp header files for installation
-install(DIRECTORY include/${PROJECT_NAME}/
+install(DIRECTORY include/${PROJECT_NAME}
   DESTINATION include
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE
diff --git a/mobility/planner_qp/traj_opt_basic/package.xml b/mobility/planner_qp/traj_opt_basic/package.xml
index c90b7f51a7..81adce94c7 100644
--- a/mobility/planner_qp/traj_opt_basic/package.xml
+++ b/mobility/planner_qp/traj_opt_basic/package.xml
@@ -17,6 +17,12 @@
 
   <build_depend>ament_cmake</build_depend>
   <build_depend>rclcpp</build_depend>
+  <build_depend>eigen3</build_depend>
 
-  <depend>eigen3</depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>eigen3</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
diff --git a/mobility/planner_qp/traj_opt_pro/CMakeLists.txt b/mobility/planner_qp/traj_opt_pro/CMakeLists.txt
index 16c9f44d4a..261de3439c 100644
--- a/mobility/planner_qp/traj_opt_pro/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_pro/CMakeLists.txt
@@ -15,7 +15,7 @@
 # License for the specific language governing permissions and limitations
 # under the License.
 
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 3.5)
 project(traj_opt_pro)
 
 # Default to C++14
@@ -44,6 +44,8 @@ endif()
 
 ## Find catkin macros and libraries
 find_package(ament_cmake REQUIRED)
+
+find_package(rclcpp REQUIRED)
 find_package(traj_opt_basic REQUIRED)
 # find_package(catkin2 REQUIRED COMPONENTS
 #   traj_opt_basic
@@ -71,6 +73,7 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
   ${OpenCV_INCLUDE_DIR}
+  ${traj_opt_basic_INCLUDE_DIRS}
 )
 
 # Add libraries
@@ -91,7 +94,7 @@ add_library(fancy_custom_backend
   src/nonlinear_trajectory.cpp)
 target_compile_definitions(${PROJECT_NAME}
   PRIVATE "COMPOSITION_BUILDING_DLL")
-target_link_libraries(fancy_custom_backend ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+target_link_libraries(fancy_custom_backend ${PROJECT_NAME} ${OpenCV_LIBRARIES} ${traj_opt_basic_LIBRARIES})
 ament_target_dependencies(${PROJECT_NAME} traj_opt_basic)
 
 #############
@@ -106,8 +109,8 @@ install(TARGETS ${PROJECT_NAME} fancy_custom_backend
 )
 
 # Mark cpp header files for installation
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+install(DIRECTORY include/${PROJECT_NAME}
+  DESTINATION include
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE
 )
diff --git a/mobility/planner_qp/traj_opt_ros/COLCON_IGNORE b/mobility/planner_qp/traj_opt_ros/COLCON_IGNORE
new file mode 100644
index 0000000000..e69de29bb2

From b4c14564a415f170b3814dfdaf1baffd86b74b7b Mon Sep 17 00:00:00 2001
From: joris997 <joris.verhagen@gmail.com>
Date: Sat, 29 Apr 2023 14:21:02 +0200
Subject: [PATCH 4/9] updates to traj_opt_ros, build errorwith ff_ros import

---
 mobility/planner_qp/_CMakeLists.txt           |  2 +-
 .../planner_qp/traj_opt_basic/CMakeLists.txt  |  2 -
 .../planner_qp/traj_opt_pro/CMakeLists.txt    |  6 +-
 .../planner_qp/traj_opt_ros/CMakeLists.txt    | 89 ++++++++++---------
 .../{COLCON_IGNORE => COLCON_IGNORE_}         |  0
 .../include/traj_opt_ros/ros_bridge.h         | 18 ++--
 .../traj_opt_ros/nodelet_plugins.xml          |  2 +-
 mobility/planner_qp/traj_opt_ros/package.xml  | 25 ++++--
 .../traj_opt_ros/src/ros_bridge.cpp           |  8 +-
 .../traj_opt_ros/src/trajectory_display.cpp   | 72 +++++++--------
 .../traj_opt_ros/src/trajectory_display.h     | 36 ++++----
 11 files changed, 135 insertions(+), 125 deletions(-)
 rename mobility/planner_qp/traj_opt_ros/{COLCON_IGNORE => COLCON_IGNORE_} (100%)

diff --git a/mobility/planner_qp/_CMakeLists.txt b/mobility/planner_qp/_CMakeLists.txt
index 080af3bc3d..2d7e920dc6 100644
--- a/mobility/planner_qp/_CMakeLists.txt
+++ b/mobility/planner_qp/_CMakeLists.txt
@@ -23,7 +23,7 @@ project(planner_qp_main)
 
 # QP planner
 add_subdirectory(traj_opt_basic)
-# add_subdirectory(traj_opt_pro)
+add_subdirectory(traj_opt_pro)
 add_subdirectory(traj_opt_msgs)
 # add_subdirectory(traj_opt_ros)
 
diff --git a/mobility/planner_qp/traj_opt_basic/CMakeLists.txt b/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
index 682e205610..4929e0970b 100644
--- a/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_basic/CMakeLists.txt
@@ -40,7 +40,6 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
   ${Boost_INCLUDE_DIRS}
-  ${traj_opt_basic_INCLUDE_DIRS}
 )
 
 add_library(${PROJECT_NAME} SHARED
@@ -52,7 +51,6 @@ add_library(${PROJECT_NAME} SHARED
 target_compile_definitions(${PROJECT_NAME}
   PRIVATE "COMPOSITION_BUILDING_DLL")
 ament_target_dependencies(${PROJECT_NAME} rclcpp)
-target_link_libraries(${PROJECT_NAME} ${traj_opt_basic_LIBRARIES})
 # ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
 
 #############
diff --git a/mobility/planner_qp/traj_opt_pro/CMakeLists.txt b/mobility/planner_qp/traj_opt_pro/CMakeLists.txt
index 261de3439c..a6844b08e7 100644
--- a/mobility/planner_qp/traj_opt_pro/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_pro/CMakeLists.txt
@@ -103,9 +103,9 @@ ament_target_dependencies(${PROJECT_NAME} traj_opt_basic)
 
 # Mark libraries for installation
 install(TARGETS ${PROJECT_NAME} fancy_custom_backend
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
 # Mark cpp header files for installation
diff --git a/mobility/planner_qp/traj_opt_ros/CMakeLists.txt b/mobility/planner_qp/traj_opt_ros/CMakeLists.txt
index 2a26865cca..9af475a9d1 100644
--- a/mobility/planner_qp/traj_opt_ros/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_ros/CMakeLists.txt
@@ -15,68 +15,65 @@
 # License for the specific language governing permissions and limitations
 # under the License.
 
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 3.5)
 project(traj_opt_ros)
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wno-deprecated-declarations  -Werror=return-type -DQT_NO_KEYWORDS")
 
 # Qt Stuff, assume Qt5
 set(CMAKE_AUTOMOC ON)
-
 ## Find catkin macros and libraries
 set(LIBRARIES traj_ros_bridge)
 
+## Find catkin macros and libraries
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(traj_opt_basic REQUIRED)
+find_package(traj_opt_msgs REQUIRED)
+
+find_package(Qt5 COMPONENTS Core)
+
 # build rviz plugin if not cross compiling
-if(NOT USE_CTC)
+if(NOT USE_CTC="${USE_CTC}")
+  message("Using CTC")
   set(CMAKE_AUTOMOC ON)
-  find_package(catkin2 COMPONENTS rviz QUIET)
-  if(rviz_QT_VERSION)
-    message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
-    find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets)
+  # find_package(catkin2 COMPONENTS rviz QUIET)
+  find_package(rviz2 QUIET)
+  if(rviz2_QT_VERSION)
+    message(STATUS "Using Qt5 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}")
+    find_package(Qt5 ${rviz2_QT_VERSION} REQUIRED Core Widgets)
     set(QT_LIBRARIES Qt5::Widgets)
     set(LIBRARIES ${LIBRARIES} traj_opt_ros)
-    set(INCLUDES ${INCLUDES} ${Qt5Core_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
+    set(INCLUDES ${INCLUDES} ${Qt5Core_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS})
   endif()
 endif()
 
-## Find catkin macros and libraries
-find_package(catkin2 REQUIRED COMPONENTS
-  roscpp
-  traj_opt_basic
-  traj_opt_msgs
-)
-
-
-catkin_package(
-  INCLUDE_DIRS include
-  LIBRARIES
-    ${LIBRARIES}
-  CATKIN_DEPENDS
-    roscpp
-    traj_opt_basic
-    traj_opt_msgs
-)
-
 ###########
 ## Build ##
 ###########
 # Specify additional locations of header files
 include_directories(
   include
-  ${catkin_INCLUDE_DIRS}
+  ${rviz2_INCLUDE_DIRS}
   ${INCLUDES}
 )
 
 add_library(traj_ros_bridge
   src/ros_bridge.cpp)
-target_link_libraries(traj_ros_bridge ${catkin_LIBRARIES})
+target_link_libraries(traj_ros_bridge ${dependencies})
 
 if (NOT USE_CTC)
-  if(rviz_QT_VERSION)
+  message("Using CTC BUILD")
+  if(rviz2_QT_VERSION)
     add_library(${PROJECT_NAME}
       src/trajectory_visual.cpp
       src/trajectory_display.cpp
       ${MOC_FILES})
-    target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES} ${rviz_DEFAULT_PLUGIN_LIBRARIES} traj_ros_bridge)
+    target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${rviz2_DEFAULT_PLUGIN_LIBRARIES} traj_ros_bridge)
   endif()
 endif (NOT USE_CTC)
 
@@ -85,31 +82,35 @@ endif (NOT USE_CTC)
 ## Install ##
 #############
 
+target_include_directories(traj_ros_bridge
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
+
 # Mark libraries for installation
 install(TARGETS traj_ros_bridge
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  EXPORT traj_ros_bridge
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
 )
 
 if (NOT USE_CTC)
-  if(rviz_QT_VERSION)
+  if(rviz2_QT_VERSION)
     install(TARGETS ${PROJECT_NAME}
-      ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-      LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-      RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+      ARCHIVE DESTINATION lib
+      LIBRARY DESTINATION lib
+      RUNTIME DESTINATION bin
     )
   endif()
 endif (NOT USE_CTC)
 
-# Mark nodelet_plugin for installation
-install(FILES nodelet_plugins.xml
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
 # Mark cpp header files for installation
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+install(DIRECTORY include/${PROJECT_NAME}
+  DESTINATION include
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE
 )
+
+ament_package()
\ No newline at end of file
diff --git a/mobility/planner_qp/traj_opt_ros/COLCON_IGNORE b/mobility/planner_qp/traj_opt_ros/COLCON_IGNORE_
similarity index 100%
rename from mobility/planner_qp/traj_opt_ros/COLCON_IGNORE
rename to mobility/planner_qp/traj_opt_ros/COLCON_IGNORE_
diff --git a/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h b/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
index a2e341dffa..800334b9b6 100644
--- a/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
+++ b/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
@@ -19,7 +19,11 @@
 #ifndef TRAJ_OPT_ROS_ROS_BRIDGE_H_
 #define TRAJ_OPT_ROS_ROS_BRIDGE_H_
 
-#include <ros/ros.h>
+// Standard includes
+#include <ff_common/ff_ros.h>
+#include <tf2_ros/buffer.h>
+#include <rclcpp/rclcpp.hpp>
+
 #include <traj_opt_basic/traj_data.h>
 #include <traj_opt_msgs/Polynomial.h>
 #include <traj_opt_msgs/Spline.h>
@@ -49,15 +53,15 @@ class TrajRosBridge {
 
   static bool are_subscribers(std::string topic);
 
-  // Use global singleton paradignm.  All these things are private!
-  // Keep your government out of my contructors!
+  // Use global singleton paradigm.  All these things are private!
+  // Keep your government out of my constructors!
  private:
   TrajRosBridge();
   static TrajRosBridge &instance();
-  static ros::Publisher getPub(std::string topic);
-  static ros::Publisher getInfoPub(std::string topic);
-  ros::NodeHandle nh_;
-  std::map<std::string, ros::Publisher> pubs_;
+  static rclcpp::Publisher getPub(std::string topic);
+  static rclcpp::Publisher getInfoPub(std::string topic);
+  rclcpp::NodeHandle nh_;
+  std::map<std::string, rclcpp::Publisher> pubs_;
 };
 
 #endif  // TRAJ_OPT_ROS_ROS_BRIDGE_H_
diff --git a/mobility/planner_qp/traj_opt_ros/nodelet_plugins.xml b/mobility/planner_qp/traj_opt_ros/nodelet_plugins.xml
index 8ee321262d..c3c7dbfa3f 100644
--- a/mobility/planner_qp/traj_opt_ros/nodelet_plugins.xml
+++ b/mobility/planner_qp/traj_opt_ros/nodelet_plugins.xml
@@ -1,7 +1,7 @@
 <library path="lib/libtraj_opt_ros">
   <class name="traj_opt_ros/Trajectory"
          type="traj_opt::TrajectoryDisplay"
-         base_class_type="rviz::Display">
+         base_class_type="rviz_common::Display">
     <description>
       View trajectory messages in rviz natively!! 
       Currently supports Mike's style and Sikang's style
diff --git a/mobility/planner_qp/traj_opt_ros/package.xml b/mobility/planner_qp/traj_opt_ros/package.xml
index e424703680..677d9bfc99 100644
--- a/mobility/planner_qp/traj_opt_ros/package.xml
+++ b/mobility/planner_qp/traj_opt_ros/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>traj_opt_ros</name>
-  <version>0.0.1</version>
+  <version>0.0.0</version>
   <description>
     The traj_opt_ros package
   </description>
@@ -14,13 +14,20 @@
   <maintainer email="astrobee-fsw@nx.arc.nasa.gov">
     Astrobee Flight Software
   </maintainer>
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>catkin_simple</build_depend>
-  <depend>rviz</depend>
-  <depend>traj_opt_basic</depend>
-  <depend>traj_opt_msgs</depend>
-  <depend>roscpp</depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rviz2</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>traj_opt_basic</build_depend>
+  <build_depend>traj_opt_msgs</build_depend>
+
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>traj_opt_basic</exec_depend>
+  <exec_depend>traj_opt_msgs</exec_depend>
+
   <export>
-    <rviz plugin="${prefix}/nodelet_plugins.xml"/>
+    <rviz2 plugin="${prefix}/nodelet_plugins.xml"/>
   </export> 
 </package>
diff --git a/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp b/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
index 7881d796f1..3f421884f2 100644
--- a/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
+++ b/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
@@ -24,14 +24,14 @@ TrajRosBridge &TrajRosBridge::instance() {
   static TrajRosBridge inst;
   return inst;
 }
-ros::Publisher TrajRosBridge::getPub(std::string topic) {
+rclcpp::Publisher TrajRosBridge::getPub(std::string topic) {
   if (instance().pubs_.find(topic) == instance().pubs_.end()) {
     instance().pubs_[topic] =
         instance().nh_.advertise<traj_opt_msgs::Trajectory>(topic, 1);
   }
   return instance().pubs_[topic];
 }
-ros::Publisher TrajRosBridge::getInfoPub(std::string topic) {
+rclcpp::Publisher TrajRosBridge::getInfoPub(std::string topic) {
   if (instance().pubs_.find(topic) == instance().pubs_.end()) {
     instance().pubs_[topic] =
         instance().nh_.advertise<traj_opt_msgs::SolverInfo>(topic, 1);
@@ -40,7 +40,7 @@ ros::Publisher TrajRosBridge::getInfoPub(std::string topic) {
 }
 
 bool TrajRosBridge::are_subscribers(std::string topic) {
-  ros::Publisher pub = getPub(topic);
+  rclcpp::Publisher pub = getPub(topic);
   return pub.getNumSubscribers() > 0;
 }
 
@@ -59,7 +59,7 @@ void TrajRosBridge::publish_msg(const traj_opt::TrajData &data,
 traj_opt_msgs::Trajectory TrajRosBridge::convert(
     const traj_opt::TrajData &data) {
   traj_opt_msgs::Trajectory traj;
-  traj.header.stamp = ros::Time::now();
+  traj.header.stamp = GetTimeNow();
   traj.header.frame_id = "map";
 
   traj.dimension_names = data.dimension_names;
diff --git a/mobility/planner_qp/traj_opt_ros/src/trajectory_display.cpp b/mobility/planner_qp/traj_opt_ros/src/trajectory_display.cpp
index bbd12b71d4..0c0a7dfc56 100644
--- a/mobility/planner_qp/traj_opt_ros/src/trajectory_display.cpp
+++ b/mobility/planner_qp/traj_opt_ros/src/trajectory_display.cpp
@@ -16,18 +16,18 @@
  * under the License.
  */
 
-#include <OGRE/OgreSceneManager.h>
-#include <OGRE/OgreSceneNode.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
 
-#include <tf/transform_listener.h>
+#include <tf2_ros/transform_listener.h>
 
-#include <rviz/frame_manager.h>
-#include <rviz/load_resource.h>
-#include <rviz/properties/color_property.h>
-#include <rviz/properties/enum_property.h>
-#include <rviz/properties/float_property.h>
-#include <rviz/properties/int_property.h>
-#include <rviz/visualization_manager.h>
+#include <rviz_common/frame_manager.hpp>
+#include <rviz_common/load_resource.hpp>
+#include <rviz_common/properties/color_property.hpp>
+#include <rviz_common/properties/enum_property.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/properties/int_property.hpp>
+#include <rviz_common/visualization_manager.hpp>
 
 #include "trajectory_display.h"  // NOLINT()
 #include "trajectory_visual.h"   // NOLINT()
@@ -38,36 +38,36 @@ namespace traj_opt {
 // The constructor must have no arguments, so we can't give the
 // constructor the parameters it needs to fully initialize.
 TrajectoryDisplay::TrajectoryDisplay() {
-  color_property_ = new rviz::ColorProperty("Color", QColor(204, 51, 204),
-                                            "Color of trajectory.", this,
-                                            SLOT(updateColorAndAlpha()));
+  color_property_ = new rviz_common::ColorProperty("Color", QColor(204, 51, 204),
+                                                  "Color of trajectory.", this,
+                                                  SLOT(updateColorAndAlpha()));
 
-  alpha_property_ = new rviz::FloatProperty(
+  alpha_property_ = new rviz_common::FloatProperty(
       "Alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", this,
       SLOT(updateColorAndAlpha()));
-  color_property_v_ = new rviz::ColorProperty(
+  color_property_v_ = new rviz_common::ColorProperty(
       "Velocity Color", QColor(20, 251, 204), "Color of velocity.", this,
       SLOT(updateColorAndAlpha()));
-  color_property_a_ = new rviz::ColorProperty(
+  color_property_a_ = new rviz_common::ColorProperty(
       "Acceleration Color", QColor(241, 21, 24), "Color of acceleration.", this,
       SLOT(updateColorAndAlpha()));
 
   thickness_property_ =
-      new rviz::FloatProperty("Line Thickness", 0.1,
-                              "Does nothing for Sikang style trajectories "
-                              "because lines are always 1px in Ogre api",
-                              this, SLOT(updateScale()));
+      new rviz_common::FloatProperty("Line Thickness", 0.1,
+                                    "Does nothing for Sikang style trajectories "
+                                   "because lines are always 1px in Ogre api",
+                                    this, SLOT(updateScale()));
   thickness_property_->setMin(0.01);
   thickness_property_->setMax(3.00);
 
   use_v_property_ =
-      new rviz::BoolProperty("Plot Velocity", true, "Turns arrow/lines on/off",
-                             this, SLOT(updateSampleLength()));
-  use_a_property_ = new rviz::BoolProperty("Plot Acceleration", false,
-                                           "Turns arrow/lines on/off", this,
-                                           SLOT(updateSampleLength()));
+      new rviz_common::BoolProperty("Plot Velocity", true, "Turns arrow/lines on/off",
+                                   this, SLOT(updateSampleLength()));
+  use_a_property_ = new rviz_common::BoolProperty("Plot Acceleration", false,
+                                                 "Turns arrow/lines on/off", this,
+                                                 SLOT(updateSampleLength()));
 
-  history_length_property_ = new rviz::IntProperty(
+  history_length_property_ = new rviz_common::IntProperty(
       "History Length", 1,
       "Number of prior trajectories to display. Warning!! "
       "Setting this too high is not recommended, it will "
@@ -76,21 +76,21 @@ TrajectoryDisplay::TrajectoryDisplay() {
   history_length_property_->setMin(1);
   history_length_property_->setMax(150);
 
-  traj_samples_property_ = new rviz::IntProperty(
+  traj_samples_property_ = new rviz_common::IntProperty(
       "Trajectory Samples", 50, "Number of samples used to draw trajectory",
       this, SLOT(updateSampleLength()));
   traj_samples_property_->setMin(10);
   traj_samples_property_->setMax(500);
 
   tangent_samples_property_ =
-      new rviz::IntProperty("Tangent Samples", 25,
-                            "Number of samples used to draw velocity and "
-                            "acceleration arrows or lines",
-                            this, SLOT(updateSampleLength()));
+      new rviz_common::IntProperty("Tangent Samples", 25,
+                                  "Number of samples used to draw velocity and "
+                                  "acceleration arrows or lines",
+                                  this, SLOT(updateSampleLength()));
   tangent_samples_property_->setMin(10);
   tangent_samples_property_->setMax(500);
 
-  style_property_ = new rviz::EnumProperty(
+  style_property_ = new rviz_common::EnumProperty(
       "Style", "Mike",
       "Mike style trajectories are plotted with cylinders and spheres. Tangent "
       "velocity/acceleration is plotted with arrows. Sikang style trajectories "
@@ -105,7 +105,7 @@ TrajectoryDisplay::TrajectoryDisplay() {
   style_property_->addOption("SE3", 3);
 }
 
-// After the top-level rviz::Display::initialize() does its own setup,
+// After the top-level rviz_common::Display::initialize() does its own setup,
 // it calls the subclass's onInitialize() function.  This is where we
 // instantiate all the workings of the class.  We make sure to also
 // call our immediate super-class's onInitialize() function, since it
@@ -124,7 +124,7 @@ void TrajectoryDisplay::onInitialize() {
   updateColorAndAlpha();
   updateSampleLength();
   //  this->setIcon(
-  //  rviz::loadPixmap("package://traj_opt_msgs/icons/classes/Trajectory.svg",true)
+  //  rviz_common::loadPixmap("package://traj_opt_msgs/icons/classes/Trajectory.svg",true)
   //  );
 }
 
@@ -190,7 +190,7 @@ void TrajectoryDisplay::updateSampleLength() {
 // This is our callback to handle an incoming message.
 void TrajectoryDisplay::processMessage(
     const traj_opt_msgs::Trajectory::ConstPtr &msg) {
-  // Here we call the rviz::FrameManager to get the transform from the
+  // Here we call the rviz_common::FrameManager to get the transform from the
   // fixed frame to the frame in the header of this Trajectory message.  If
   // it fails, we can't do anything else so we return.
   Ogre::Quaternion orientation;
@@ -245,4 +245,4 @@ void TrajectoryDisplay::randomizeColor() {}
 // Tell pluginlib about this class.  It is important to do this in
 // global scope, outside our package's namespace.
 #include <pluginlib/class_list_macros.h>  // NOLINT()
-PLUGINLIB_EXPORT_CLASS(traj_opt::TrajectoryDisplay, rviz::Display)
+PLUGINLIB_EXPORT_CLASS(traj_opt::TrajectoryDisplay, rviz_common::Display)
diff --git a/mobility/planner_qp/traj_opt_ros/src/trajectory_display.h b/mobility/planner_qp/traj_opt_ros/src/trajectory_display.h
index 9fab9b507e..b7cd179a2b 100644
--- a/mobility/planner_qp/traj_opt_ros/src/trajectory_display.h
+++ b/mobility/planner_qp/traj_opt_ros/src/trajectory_display.h
@@ -20,7 +20,7 @@
 #define MOBILITY_PLANNER_QP_TRAJ_OPT_ROS_SRC_TRAJECTORY_DISPLAY_H_
 
 #ifndef Q_MOC_RUN
-#include <rviz/message_filter_display.h>
+#include <rviz_common/message_filter_display.h>
 #include <traj_opt_msgs/Trajectory.h>
 #include <boost/circular_buffer.hpp>
 #endif
@@ -29,7 +29,7 @@ namespace Ogre {
 class SceneNode;
 }
 
-namespace rviz {
+namespace rviz_common {
 class ColorProperty;
 class FloatProperty;
 class IntProperty;
@@ -44,9 +44,9 @@ namespace traj_opt {
 class TrajectoryVisual;
 
 // BEGIN_TUTORIAL
-// Here we declare our new subclass of rviz::Display.  Every display
+// Here we declare our new subclass of rviz_common::Display.  Every display
 // which can be listed in the "Displays" panel is a subclass of
-// rviz::Display.
+// rviz_common::Display.
 //
 // TrajectoryDisplay will show a 3D arrow showing the direction and magnitude
 // of the TRAJECTORY acceleration vector.  The base of the arrow will be at
@@ -61,7 +61,7 @@ class TrajectoryVisual;
 // idiom for the visuals is that when the objects exist, they appear
 // in the scene, and when they are deleted, they disappear.
 class TrajectoryDisplay
-    : public rviz::MessageFilterDisplay<traj_opt_msgs::Trajectory> {
+    : public rviz_common::MessageFilterDisplay<traj_opt_msgs::Trajectory> {
   Q_OBJECT  // NOLINT
       public
       :  // NOLINT
@@ -99,19 +99,19 @@ class TrajectoryDisplay
   boost::circular_buffer<boost::shared_ptr<TrajectoryVisual> > visuals_;
 
   // User-editable property variables.
-  rviz::ColorProperty* color_property_;
-  rviz::FloatProperty* alpha_property_;
-  rviz::ColorProperty* color_property_v_;
-  rviz::ColorProperty* color_property_a_;
-
-  rviz::BoolProperty* use_v_property_;
-  rviz::BoolProperty* use_a_property_;
-
-  rviz::FloatProperty* thickness_property_;
-  rviz::IntProperty* history_length_property_;
-  rviz::IntProperty* traj_samples_property_;
-  rviz::IntProperty* tangent_samples_property_;
-  rviz::EnumProperty* style_property_;
+  rviz_common::ColorProperty* color_property_;
+  rviz_common::FloatProperty* alpha_property_;
+  rviz_common::ColorProperty* color_property_v_;
+  rviz_common::ColorProperty* color_property_a_;
+
+  rviz_common::BoolProperty* use_v_property_;
+  rviz_common::BoolProperty* use_a_property_;
+
+  rviz_common::FloatProperty* thickness_property_;
+  rviz_common::IntProperty* history_length_property_;
+  rviz_common::IntProperty* traj_samples_property_;
+  rviz_common::IntProperty* tangent_samples_property_;
+  rviz_common::EnumProperty* style_property_;
 };
 // END_TUTORIAL
 

From dc228f50c3bc6cb3bc712714d6bb68f5ba9c759f Mon Sep 17 00:00:00 2001
From: joris997 <joris.verhagen@gmail.com>
Date: Thu, 11 May 2023 12:41:53 +0000
Subject: [PATCH 5/9] work on ros_bridge, writing as two speerate instances

---
 .../planner_qp/traj_opt_msgs/CMakeLists.txt   |  2 +
 .../planner_qp/traj_opt_ros/CMakeLists.txt    | 63 ++++++++++---------
 .../include/traj_opt_ros/ros_bridge.h         | 38 ++++++++---
 mobility/planner_qp/traj_opt_ros/package.xml  |  2 +
 .../traj_opt_ros/src/ros_bridge.cpp           | 38 ++++++-----
 5 files changed, 91 insertions(+), 52 deletions(-)

diff --git a/mobility/planner_qp/traj_opt_msgs/CMakeLists.txt b/mobility/planner_qp/traj_opt_msgs/CMakeLists.txt
index 417a5a4161..7e030cdfce 100644
--- a/mobility/planner_qp/traj_opt_msgs/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_msgs/CMakeLists.txt
@@ -30,6 +30,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(ff_util REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 # Generate messages in the 'msg' folder
@@ -45,6 +46,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   DEPENDENCIES
   builtin_interfaces
   std_msgs
+  ff_util
 
   ADD_LINTER_TESTS
 )
diff --git a/mobility/planner_qp/traj_opt_ros/CMakeLists.txt b/mobility/planner_qp/traj_opt_ros/CMakeLists.txt
index 9af475a9d1..0697175bd9 100644
--- a/mobility/planner_qp/traj_opt_ros/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_ros/CMakeLists.txt
@@ -24,33 +24,37 @@ endif()
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wno-deprecated-declarations  -Werror=return-type -DQT_NO_KEYWORDS")
 
-# Qt Stuff, assume Qt5
+## Qt Stuff, assume Qt5
 set(CMAKE_AUTOMOC ON)
+
 ## Find catkin macros and libraries
 set(LIBRARIES traj_ros_bridge)
 
-## Find catkin macros and libraries
+## Find amend macros and libraries
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+
+find_package(ff_common REQUIRED)
+find_package(ff_util REQUIRED)
 find_package(traj_opt_basic REQUIRED)
 find_package(traj_opt_msgs REQUIRED)
 
 find_package(Qt5 COMPONENTS Core)
 
 # build rviz plugin if not cross compiling
-if(NOT USE_CTC="${USE_CTC}")
-  message("Using CTC")
-  set(CMAKE_AUTOMOC ON)
-  # find_package(catkin2 COMPONENTS rviz QUIET)
-  find_package(rviz2 QUIET)
-  if(rviz2_QT_VERSION)
-    message(STATUS "Using Qt5 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}")
-    find_package(Qt5 ${rviz2_QT_VERSION} REQUIRED Core Widgets)
-    set(QT_LIBRARIES Qt5::Widgets)
-    set(LIBRARIES ${LIBRARIES} traj_opt_ros)
-    set(INCLUDES ${INCLUDES} ${Qt5Core_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS})
-  endif()
-endif()
+# if(NOT USE_CTC="${USE_CTC}")
+#   # message("Using CTC")
+#   set(CMAKE_AUTOMOC ON)
+#   # find_package(catkin2 COMPONENTS rviz QUIET)
+#   find_package(rviz2 QUIET)
+#   if(rviz2_QT_VERSION)
+#     message(STATUS "Using Qt5 based on the rviz2_QT_VERSION: ${rviz2_QT_VERSION}")
+#     find_package(Qt5 ${rviz2_QT_VERSION} REQUIRED Core Widgets)
+#     set(QT_LIBRARIES Qt5::Widgets)
+#     set(LIBRARIES ${LIBRARIES} traj_opt_ros)
+#     set(INCLUDES ${INCLUDES} ${Qt5Core_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS})
+#   endif()
+# endif()
 
 ###########
 ## Build ##
@@ -63,19 +67,22 @@ include_directories(
 )
 
 add_library(traj_ros_bridge
-  src/ros_bridge.cpp)
-target_link_libraries(traj_ros_bridge ${dependencies})
-
-if (NOT USE_CTC)
-  message("Using CTC BUILD")
-  if(rviz2_QT_VERSION)
-    add_library(${PROJECT_NAME}
-      src/trajectory_visual.cpp
-      src/trajectory_display.cpp
-      ${MOC_FILES})
-    target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${rviz2_DEFAULT_PLUGIN_LIBRARIES} traj_ros_bridge)
-  endif()
-endif (NOT USE_CTC)
+            src/ros_bridge.cpp)
+target_link_libraries(traj_ros_bridge ff_common ff_util traj_opt_basic traj_opt_msgs)
+
+# if (NOT USE_CTC)
+#   message("Using CTC BUILD")
+#   if(rviz2_QT_VERSION)
+#     add_library(${PROJECT_NAME}
+#       src/trajectory_visual.cpp
+#       src/trajectory_display.cpp
+#       ${MOC_FILES})
+#     target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${rviz2_DEFAULT_PLUGIN_LIBRARIES} traj_ros_bridge)
+#   endif()
+# endif (NOT USE_CTC)
+
+ament_target_dependencies(traj_ros_bridge ${QT_LIBRARIES} ff_common rclcpp traj_opt_basic traj_opt_msgs)
+ament_export_targets(traj_ros_bridge HAS_LIBRARY_TARGET)
 
 
 #############
diff --git a/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h b/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
index 800334b9b6..53ceb4f316 100644
--- a/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
+++ b/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
@@ -25,13 +25,21 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <traj_opt_basic/traj_data.h>
-#include <traj_opt_msgs/Polynomial.h>
-#include <traj_opt_msgs/Spline.h>
-#include <traj_opt_msgs/Trajectory.h>
-#include <traj_opt_msgs/SolverInfo.h>
+#include <traj_opt_msgs/msg/polynomial.hpp>
+#include <traj_opt_msgs/msg/spline.hpp>
+#include <traj_opt_msgs/msg/trajectory.hpp>
+#include <traj_opt_msgs/msg/solver_info.hpp>
 #include <string>
 #include <map>
 
+namespace traj_opt_msgs {
+  typedef msg::Polynomial Polynomial;
+  typedef msg::Spline Spline;
+  typedef msg::Trajectory Trajectory;
+  typedef msg::SolverInfo SolverInfo;
+} // namespace traj_opt_msgs
+
+
 class TrajRosBridge {
  public:
   // No need to instantiate pesky variables!
@@ -58,10 +66,24 @@ class TrajRosBridge {
  private:
   TrajRosBridge();
   static TrajRosBridge &instance();
-  static rclcpp::Publisher getPub(std::string topic);
-  static rclcpp::Publisher getInfoPub(std::string topic);
-  rclcpp::NodeHandle nh_;
-  std::map<std::string, rclcpp::Publisher> pubs_;
+
+  static rclcpp::Publisher<traj_opt_msgs::Trajectory>::SharedPtr getPub(std::string topic);
+  static rclcpp::Publisher<traj_opt_msgs::SolverInfo>::SharedPtr getInfoPub(std::string topic);
+
+  NodeHandle nh_traj_;
+  NodeHandle nh_info_;
+  std::map<std::string, rclcpp::Publisher<traj_opt_msgs::Trajectory>::SharedPtr> pubs_traj_;
+  std::map<std::string, rclcpp::Publisher<traj_opt_msgs::SolverInfo>::SharedPtr> pubs_info_;
+
+  // static rclcpp::PublisherBase::SharedPtr getPub(std::string topic);
+  // static rclcpp::PublisherBase::SharedPtr getInfoPub(std::string topic);
+
+  // static rclcpp::PublisherBase::SharedPtr getPub(std::string topic);
+  // static rclcpp::PublisherBase::SharedPtr getInfoPub(std::string topic);
+
+  // // rclcpp::node::Node::make_shared nh_("~");
+  // NodeHandle nh_;
+  // std::map<std::string, rclcpp::PublisherBase::SharedPtr> pubs_;
 };
 
 #endif  // TRAJ_OPT_ROS_ROS_BRIDGE_H_
diff --git a/mobility/planner_qp/traj_opt_ros/package.xml b/mobility/planner_qp/traj_opt_ros/package.xml
index 677d9bfc99..4133f78390 100644
--- a/mobility/planner_qp/traj_opt_ros/package.xml
+++ b/mobility/planner_qp/traj_opt_ros/package.xml
@@ -21,11 +21,13 @@
   <build_depend>rclcpp</build_depend>
   <build_depend>traj_opt_basic</build_depend>
   <build_depend>traj_opt_msgs</build_depend>
+  <build_depend>ff_util</build_depend>
 
   <exec_depend>rviz2</exec_depend>
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>traj_opt_basic</exec_depend>
   <exec_depend>traj_opt_msgs</exec_depend>
+  <exec_depend>ff_util</exec_depend>
 
   <export>
     <rviz2 plugin="${prefix}/nodelet_plugins.xml"/>
diff --git a/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp b/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
index 3f421884f2..0b80fc80f1 100644
--- a/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
+++ b/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
@@ -17,38 +17,44 @@
  */
 
 #include <traj_opt_ros/ros_bridge.h>
+#include <ff_util/ff_component.h>
 #include <string>
 
-TrajRosBridge::TrajRosBridge() : nh_("~") {}
+TrajRosBridge::TrajRosBridge(){
+  nh_traj_ = std::make_shared<rclcpp::Node>("~","~");
+  nh_info_ = std::make_shared<rclcpp::Node>("~","~");
+  // rclcpp::Node n("~");
+  // nh_ = std::shared_ptr(n);
+}
 TrajRosBridge &TrajRosBridge::instance() {
   static TrajRosBridge inst;
   return inst;
 }
-rclcpp::Publisher TrajRosBridge::getPub(std::string topic) {
-  if (instance().pubs_.find(topic) == instance().pubs_.end()) {
-    instance().pubs_[topic] =
-        instance().nh_.advertise<traj_opt_msgs::Trajectory>(topic, 1);
+rclcpp::Publisher<traj_opt_msgs::Trajectory>::SharedPtr TrajRosBridge::getPub(std::string topic) {
+  if (instance().pubs_traj_.find(topic) == instance().pubs_traj_.end()) {
+    instance().pubs_traj_[topic] =
+        instance().nh_traj_->create_publisher<traj_opt_msgs::Trajectory>(topic, 1);
   }
-  return instance().pubs_[topic];
+  return instance().pubs_traj_[topic];
 }
-rclcpp::Publisher TrajRosBridge::getInfoPub(std::string topic) {
-  if (instance().pubs_.find(topic) == instance().pubs_.end()) {
-    instance().pubs_[topic] =
-        instance().nh_.advertise<traj_opt_msgs::SolverInfo>(topic, 1);
+rclcpp::Publisher<traj_opt_msgs::SolverInfo>::SharedPtr TrajRosBridge::getInfoPub(std::string topic) {
+  if (instance().pubs_info_.find(topic) == instance().pubs_info_.end()) {
+    instance().pubs_info_[topic] =
+        instance().nh_info_->create_publisher<traj_opt_msgs::SolverInfo>(topic, 1);
   }
-  return instance().pubs_[topic];
+  return instance().pubs_info_[topic];
 }
 
 bool TrajRosBridge::are_subscribers(std::string topic) {
-  rclcpp::Publisher pub = getPub(topic);
-  return pub.getNumSubscribers() > 0;
+  rclcpp::PublisherBase::SharedPtr pub = getPub(topic);
+  return pub->get_subscription_count() > 0;
 }
 
 void TrajRosBridge::publish_msg(const traj_opt_msgs::Trajectory &msg,
                                 std::string frame_id, std::string topic) {
   traj_opt_msgs::Trajectory msgc = msg;
   msgc.header.frame_id = frame_id;
-  getPub(topic).publish(msgc);
+  getPub(topic)->publish(msgc);
 }
 void TrajRosBridge::publish_msg(const traj_opt::TrajData &data,
                                 std::string frame_id, std::string topic) {
@@ -59,7 +65,7 @@ void TrajRosBridge::publish_msg(const traj_opt::TrajData &data,
 traj_opt_msgs::Trajectory TrajRosBridge::convert(
     const traj_opt::TrajData &data) {
   traj_opt_msgs::Trajectory traj;
-  traj.header.stamp = GetTimeNow();
+  traj.header.stamp = GetTimeNow(); //nh_traj_->get_clock()->now(); //
   traj.header.frame_id = "map";
 
   traj.dimension_names = data.dimension_names;
@@ -119,7 +125,7 @@ traj_opt_msgs::SolverInfo TrajRosBridge::convert(
 
 void TrajRosBridge::publish_msg(const traj_opt_msgs::SolverInfo &msg,
                                 std::string topic) {
-  getInfoPub(topic).publish(msg);
+  getInfoPub(topic)->publish(msg);
 }
 void TrajRosBridge::publish_msg(const traj_opt::SolverInfo &data,
                                 std::string topic) {

From 0aa90c166386aabd3c19b049014c8e13c6b5ff1f Mon Sep 17 00:00:00 2001
From: joris997 <joris.verhagen@gmail.com>
Date: Wed, 17 May 2023 19:03:11 +0200
Subject: [PATCH 6/9] fixed clock not defined in class definition

---
 mobility/planner_qp/_CMakeLists.txt           | 32 --------
 .../planner_qp/traj_opt_ros/CMakeLists.txt    | 74 +++++++++++--------
 .../planner_qp/traj_opt_ros/COLCON_IGNORE_    |  0
 .../include/traj_opt_ros/ros_bridge.h         | 13 +---
 mobility/planner_qp/traj_opt_ros/package.xml  |  2 +
 .../traj_opt_ros/src/ros_bridge.cpp           |  3 +-
 6 files changed, 50 insertions(+), 74 deletions(-)
 delete mode 100644 mobility/planner_qp/_CMakeLists.txt
 delete mode 100644 mobility/planner_qp/traj_opt_ros/COLCON_IGNORE_

diff --git a/mobility/planner_qp/_CMakeLists.txt b/mobility/planner_qp/_CMakeLists.txt
deleted file mode 100644
index 2d7e920dc6..0000000000
--- a/mobility/planner_qp/_CMakeLists.txt
+++ /dev/null
@@ -1,32 +0,0 @@
-# Copyright (c) 2017, United States Government, as represented by the
-# Administrator of the National Aeronautics and Space Administration.
-# 
-# All rights reserved.
-# 
-# The Astrobee platform is licensed under the Apache License, Version 2.0
-# (the "License"); you may not use this file except in compliance with the
-# License. You may obtain a copy of the License at
-# 
-#     http://www.apache.org/licenses/LICENSE-2.0
-# 
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
-# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
-# License for the specific language governing permissions and limitations
-# under the License.
-cmake_minimum_required(VERSION 3.5)
-project(planner_qp_main)
-# this flag is needed by CGAL
-# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -frounding-math")
-# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -frounding-math")
-# set(NASA ON CACHE BOOL "Use NASA's wonderful build system on all traj_opt packages" FORCE)
-
-# QP planner
-add_subdirectory(traj_opt_basic)
-add_subdirectory(traj_opt_pro)
-add_subdirectory(traj_opt_msgs)
-# add_subdirectory(traj_opt_ros)
-
-# add_subdirectory(planner_qp)
-
-ament_package()
diff --git a/mobility/planner_qp/traj_opt_ros/CMakeLists.txt b/mobility/planner_qp/traj_opt_ros/CMakeLists.txt
index 0697175bd9..5a5c9b495b 100644
--- a/mobility/planner_qp/traj_opt_ros/CMakeLists.txt
+++ b/mobility/planner_qp/traj_opt_ros/CMakeLists.txt
@@ -17,6 +17,7 @@
 
 cmake_minimum_required(VERSION 3.5)
 project(traj_opt_ros)
+
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
@@ -24,23 +25,30 @@ endif()
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wno-deprecated-declarations  -Werror=return-type -DQT_NO_KEYWORDS")
 
-## Qt Stuff, assume Qt5
-set(CMAKE_AUTOMOC ON)
+# ## Qt Stuff, assume Qt5
+# set(CMAKE_AUTOMOC ON)
 
-## Find catkin macros and libraries
-set(LIBRARIES traj_ros_bridge)
+# ## Find catkin macros and libraries
+# set(LIBRARIES traj_ros_bridge)
 
 ## Find amend macros and libraries
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-
-find_package(ff_common REQUIRED)
-find_package(ff_util REQUIRED)
-find_package(traj_opt_basic REQUIRED)
-find_package(traj_opt_msgs REQUIRED)
+set(dependencies 
+rclcpp
+rclcpp_components
+ff_common
+ff_msgs
+ff_util
+traj_opt_basic
+traj_opt_msgs
+)
 
+find_package(ament_cmake REQUIRED)
 find_package(Qt5 COMPONENTS Core)
 
+foreach(dep ${dependencies})
+  find_package(${dep} REQUIRED)
+endforeach()
+
 # build rviz plugin if not cross compiling
 # if(NOT USE_CTC="${USE_CTC}")
 #   # message("Using CTC")
@@ -67,8 +75,14 @@ include_directories(
 )
 
 add_library(traj_ros_bridge
-            src/ros_bridge.cpp)
-target_link_libraries(traj_ros_bridge ff_common ff_util traj_opt_basic traj_opt_msgs)
+  src/ros_bridge.cpp
+)
+
+ament_target_dependencies(traj_ros_bridge ${QT_LIBRARIES} ${dependencies})
+
+target_compile_definitions(traj_ros_bridge PRIVATE "COMPOSITION_BUILDING_DLL")
+target_link_libraries(traj_ros_bridge ${dependencies})
+ament_export_targets(traj_ros_bridge HAS_LIBRARY_TARGET)
 
 # if (NOT USE_CTC)
 #   message("Using CTC BUILD")
@@ -81,18 +95,14 @@ target_link_libraries(traj_ros_bridge ff_common ff_util traj_opt_basic traj_opt_
 #   endif()
 # endif (NOT USE_CTC)
 
-ament_target_dependencies(traj_ros_bridge ${QT_LIBRARIES} ff_common rclcpp traj_opt_basic traj_opt_msgs)
-ament_export_targets(traj_ros_bridge HAS_LIBRARY_TARGET)
-
-
 #############
 ## Install ##
 #############
 
-target_include_directories(traj_ros_bridge
-  PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>)
+# target_include_directories(traj_ros_bridge
+#   PUBLIC
+#     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#     $<INSTALL_INTERFACE:include>)
 
 # Mark libraries for installation
 install(TARGETS traj_ros_bridge
@@ -103,19 +113,19 @@ install(TARGETS traj_ros_bridge
   INCLUDES DESTINATION include
 )
 
-if (NOT USE_CTC)
-  if(rviz2_QT_VERSION)
-    install(TARGETS ${PROJECT_NAME}
-      ARCHIVE DESTINATION lib
-      LIBRARY DESTINATION lib
-      RUNTIME DESTINATION bin
-    )
-  endif()
-endif (NOT USE_CTC)
+# if (NOT USE_CTC)
+#   if(rviz2_QT_VERSION)
+#     install(TARGETS ${PROJECT_NAME}
+#       ARCHIVE DESTINATION lib
+#       LIBRARY DESTINATION lib
+#       RUNTIME DESTINATION bin
+#     )
+#   endif()
+# endif (NOT USE_CTC)
 
 # Mark cpp header files for installation
-install(DIRECTORY include/${PROJECT_NAME}
-  DESTINATION include
+install(DIRECTORY launch/
+  DESTINATION share/traj_ros_bridge
   FILES_MATCHING PATTERN "*.h"
   PATTERN ".svn" EXCLUDE
 )
diff --git a/mobility/planner_qp/traj_opt_ros/COLCON_IGNORE_ b/mobility/planner_qp/traj_opt_ros/COLCON_IGNORE_
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h b/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
index 53ceb4f316..769abdea5e 100644
--- a/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
+++ b/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
@@ -24,6 +24,8 @@
 #include <tf2_ros/buffer.h>
 #include <rclcpp/rclcpp.hpp>
 
+#include <ff_util/ff_component.h>
+
 #include <traj_opt_basic/traj_data.h>
 #include <traj_opt_msgs/msg/polynomial.hpp>
 #include <traj_opt_msgs/msg/spline.hpp>
@@ -67,6 +69,8 @@ class TrajRosBridge {
   TrajRosBridge();
   static TrajRosBridge &instance();
 
+  static rclcpp::Clock::SharedPtr clock_;
+
   static rclcpp::Publisher<traj_opt_msgs::Trajectory>::SharedPtr getPub(std::string topic);
   static rclcpp::Publisher<traj_opt_msgs::SolverInfo>::SharedPtr getInfoPub(std::string topic);
 
@@ -75,15 +79,6 @@ class TrajRosBridge {
   std::map<std::string, rclcpp::Publisher<traj_opt_msgs::Trajectory>::SharedPtr> pubs_traj_;
   std::map<std::string, rclcpp::Publisher<traj_opt_msgs::SolverInfo>::SharedPtr> pubs_info_;
 
-  // static rclcpp::PublisherBase::SharedPtr getPub(std::string topic);
-  // static rclcpp::PublisherBase::SharedPtr getInfoPub(std::string topic);
-
-  // static rclcpp::PublisherBase::SharedPtr getPub(std::string topic);
-  // static rclcpp::PublisherBase::SharedPtr getInfoPub(std::string topic);
-
-  // // rclcpp::node::Node::make_shared nh_("~");
-  // NodeHandle nh_;
-  // std::map<std::string, rclcpp::PublisherBase::SharedPtr> pubs_;
 };
 
 #endif  // TRAJ_OPT_ROS_ROS_BRIDGE_H_
diff --git a/mobility/planner_qp/traj_opt_ros/package.xml b/mobility/planner_qp/traj_opt_ros/package.xml
index 4133f78390..09f6eb5198 100644
--- a/mobility/planner_qp/traj_opt_ros/package.xml
+++ b/mobility/planner_qp/traj_opt_ros/package.xml
@@ -22,12 +22,14 @@
   <build_depend>traj_opt_basic</build_depend>
   <build_depend>traj_opt_msgs</build_depend>
   <build_depend>ff_util</build_depend>
+  <build_depend>ff_msgs</build_depend>
 
   <exec_depend>rviz2</exec_depend>
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>traj_opt_basic</exec_depend>
   <exec_depend>traj_opt_msgs</exec_depend>
   <exec_depend>ff_util</exec_depend>
+  <exec_depend>ff_msgs</exec_depend>
 
   <export>
     <rviz2 plugin="${prefix}/nodelet_plugins.xml"/>
diff --git a/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp b/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
index 0b80fc80f1..bed14a0d51 100644
--- a/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
+++ b/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
@@ -65,7 +65,8 @@ void TrajRosBridge::publish_msg(const traj_opt::TrajData &data,
 traj_opt_msgs::Trajectory TrajRosBridge::convert(
     const traj_opt::TrajData &data) {
   traj_opt_msgs::Trajectory traj;
-  traj.header.stamp = GetTimeNow(); //nh_traj_->get_clock()->now(); //
+
+  traj.header.stamp = clock_->now();
   traj.header.frame_id = "map";
 
   traj.dimension_names = data.dimension_names;

From e1a1388b697a97955b26388ce8dbe1edfea447ac Mon Sep 17 00:00:00 2001
From: joris997 <joris.verhagen@gmail.com>
Date: Wed, 31 May 2023 14:10:46 +0200
Subject: [PATCH 7/9] fixed lint errors

---
 astrobee/launch/granite.launch.py             | 210 ++++++++++++++++++
 .../include/traj_opt_ros/ros_bridge.h         |   5 +-
 .../traj_opt_ros/src/ros_bridge.cpp           |   6 +-
 3 files changed, 215 insertions(+), 6 deletions(-)
 create mode 100644 astrobee/launch/granite.launch.py

diff --git a/astrobee/launch/granite.launch.py b/astrobee/launch/granite.launch.py
new file mode 100644
index 0000000000..8734cdec9c
--- /dev/null
+++ b/astrobee/launch/granite.launch.py
@@ -0,0 +1,210 @@
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+#
+# All rights reserved.
+#
+# The Astrobee platform is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+
+
+from utilities.utilities import *
+
+def generate_launch_description():
+    print("asdf")
+    robot_description = Command(['xacro ', get_path('urdf/model.urdf.xacro', 'description'),
+                        ' world:="',      LaunchConfiguration('world'),
+                        '" top_aft:="',   LaunchConfiguration('top_aft'),
+                        '" bot_aft:="',   LaunchConfiguration('bot_aft'),
+                        '" bot_front:="', LaunchConfiguration('bot_front'), 
+                        '" ns:="_',       LaunchConfiguration('ns'),
+                        '" prefix:="',    LaunchConfiguration('ns'), '/"' ])
+    # robot_description = Command(['xacro ', get_path('urdf/model_carriage.urdf.xacro')])
+    print("asdf")
+
+    return LaunchDescription([
+        # Context options (NB: THESE ARE OVERRIDDEN BY ENVIRONMENT VARIABLES)   -->
+        # Set world and world correctly; environment variable over rule default -->
+        DeclareLaunchArgument("robot", default_value=os.getenv("ASTROBEE_ROBOT", "bsharp")),
+        DeclareLaunchArgument("world", default_value=os.getenv("ASTROBEE_WORLD", "granite")),
+
+        DeclareLaunchArgument("ns",     default_value=""),     # Robot namespace
+        DeclareLaunchArgument("spurn",  default_value=""),     # Prevent a specific node
+        DeclareLaunchArgument("nodes",  default_value=""),     # Launch specific nodes
+        DeclareLaunchArgument("extra",  default_value=""),     # Inject an additional node
+        DeclareLaunchArgument("debug",  default_value=""),     # Debug node group
+        DeclareLaunchArgument("dds",    default_value="true"), # Enable DDS
+        
+        # General options
+        DeclareLaunchArgument("vive",   default_value="false"),
+        DeclareLaunchArgument("gviz",   default_value="false"),
+        DeclareLaunchArgument("rviz",   default_value="false"),
+        DeclareLaunchArgument("sviz",   default_value="false"),
+        DeclareLaunchArgument("rqt",    default_value="false"),
+        DeclareLaunchArgument("gds",    default_value="false"),
+        DeclareLaunchArgument("agent1", default_value="Bsharp"),
+        DeclareLaunchArgument("agent2", default_value="Bumble"),
+        DeclareLaunchArgument("agent3", default_value="Honey"),
+        DeclareLaunchArgument("stats",  default_value="false"),
+
+        # Remaining options
+        DeclareLaunchArgument("output",  default_value="log"),    # Output to screen or log
+        DeclareLaunchArgument("gtloc",   default_value="false"),  # Use Ground Truth Localizer
+        DeclareLaunchArgument("drivers", default_value="true"),   # Should we launch drivers?
+        DeclareLaunchArgument("llp",     default_value="10.42.0.37"),  # LLP IP address
+        DeclareLaunchArgument("mlp",     default_value="10.42.0.38"),  # MLP IP address
+        DeclareLaunchArgument("rec",     default_value=""),
+        DeclareLaunchArgument("vmware",  default_value="true"),
+        DeclareLaunchArgument("speed",   default_value="1"),
+        DeclareLaunchArgument("physics", default_value="ode"),
+        DeclareLaunchArgument("sim",     default_value="local"),
+
+
+        # Simulation options only -->
+        DeclareLaunchArgument("pose", default_value="0 0 -0.15 0 0 0"),  # Initial pose (sim only)
+
+        # Path to the bag file
+        DeclareLaunchArgument("bag", default_value=""),
+
+        # Make sure all environment variables are set for controller
+        # Override the robot and world environment variables all the time. The
+        # environment variables are the default if they are set. So in this
+        # case we are overriding the environment variables with themselves.
+        # Roslaunch arguments override the environment variable which is what
+        # this will do.
+        SetEnvironmentVariable(
+            name='ASTROBEE_ROBOT',
+            value=LaunchConfiguration('robot'),
+        ),
+        SetEnvironmentVariable(
+            name='ASTROBEE_WORLD',
+            value=LaunchConfiguration('world'),
+        ),
+        SetEnvironmentVariable(
+            name='ASTROBEE_CONFIG_DIR',
+            value=os.getenv("ASTROBEE_CONFIG_DIR",get_path("config")),
+        ),
+        SetEnvironmentVariable(
+            name='ASTROBEE_RESOURCE_DIR',
+            value=os.getenv("ASTROBEE_RESOURCE_DIR",get_path("resources")),
+        ),
+        SetEnvironmentVariable(
+            name='ROSCONSOLE_CONFIG_FILE',
+            value=os.getenv("ROSCONSOLE_CONFIG_FILE",get_path("resources/logging.config")),
+        ),
+
+        SetEnvironmentVariable(
+            name='ROSCONSOLE_FORMAT',
+            value="[${severity}] [${time}] : (${logger}) ${message}"
+        ),
+
+        # Start ground controller services
+        IncludeLaunchDescription(
+            get_launch_file("launch/controller/descriptions.launch.py"),
+            launch_arguments={
+                "world": LaunchConfiguration("world"),
+            }.items(),
+        ),
+        IncludeLaunchDescription(
+            get_launch_file("launch/controller/stats.launch.py"),
+            condition=IfCondition(LaunchConfiguration("stats")),
+        ),
+        IncludeLaunchDescription(
+            get_launch_file("launch/controller/vive.launch.py"),
+            condition=IfCondition(LaunchConfiguration("vive")),
+        ),
+        IncludeLaunchDescription(
+            get_launch_file("launch/controller/gviz.launch.py"),
+            condition=IfCondition(LaunchConfiguration("gviz")),
+        ),
+        IncludeLaunchDescription(
+            get_launch_file("launch/controller/gds.launch.py"),
+            launch_arguments={
+                "world" : LaunchConfiguration("world"),
+                "agent1": LaunchConfiguration("agent1"),
+                "agent2": LaunchConfiguration("agent2"),
+                "agent3": LaunchConfiguration("agent3"),
+            }.items(),
+            condition=IfCondition(LaunchConfiguration("gds")),
+        ),
+        IncludeLaunchDescription(
+            get_launch_file("launch/controller/rqt.launch.py"),
+            condition=IfCondition(LaunchConfiguration("rqt")),
+        ),
+        IncludeLaunchDescription(
+            get_launch_file("launch/controller/rviz.launch.py"),
+            launch_arguments={
+                "world" : LaunchConfiguration("world"),
+            }.items(),
+            condition=IfCondition(LaunchConfiguration("rviz")),
+        ),
+        # Launch a recorder for this robot
+        IncludeLaunchDescription(
+            get_launch_file("launch/controller/bagrecord.launch.py"),
+            condition=LaunchConfigurationNotEquals("rec", ""),
+            launch_arguments={"bag": LaunchConfiguration("rec")}.items(),
+        ),
+
+        # Start the simulator
+        IncludeLaunchDescription(
+            get_launch_file("launch/controller/sim_start.launch.py"),
+            launch_arguments={
+                "world"  : LaunchConfiguration("world"),
+                "sviz"   : LaunchConfiguration("sviz"),
+                "vmware" : LaunchConfiguration("vmware"),
+                "speed"  : LaunchConfiguration("speed"),
+                "debug"  : LaunchConfiguration("debug"),
+                "physics": LaunchConfiguration("physics"),
+            }.items(),
+        ),
+
+        # # Auto-inert platform #1 at a desired initial location
+        # IncludeLaunchDescription(
+        #     get_launch_file("launch/spawn.launch.py"),
+        #     launch_arguments={
+        #         "robot" : LaunchConfiguration("robot"),      # Type of robot
+        #         "world" : LaunchConfiguration("world"),      # Execution context
+        #         "ns"    : LaunchConfiguration("ns"),         # Robot namespace
+        #         "output": LaunchConfiguration("output"),     # Output for logging
+        #         "pose"  : LaunchConfiguration("pose"),       # Initial robot pose
+        #         "spurn" : LaunchConfiguration("spurn"),      # Prevent node
+        #         "nodes" : LaunchConfiguration("nodes"),      # Launch node group
+        #         "extra" : LaunchConfiguration("extra"),      # Inject extra nodes
+        #         "debug" : LaunchConfiguration("debug"),      # Debug a node set
+        #         "sim"   : LaunchConfiguration("sim"),        # SIM IP address
+        #         "llp"   : LaunchConfiguration("llp"),        # LLP IP address
+        #         "mlp"   : LaunchConfiguration("mlp"),        # MLP IP address
+        #         "dds"   : LaunchConfiguration("dds"),        # Enable DDS
+        #         "gtloc" : LaunchConfiguration("gtloc"),      # Use Ground Truth Localizer
+        #     }.items(),
+        # ),
+
+        # # bsharp
+        # IncludeLaunchDescription(
+        #     get_launch_file("launch/astrobee.launch.py"),
+        #     launch_arguments={
+        #         "robot"  : LaunchConfiguration("robot"),
+        #         "world"  : LaunchConfiguration("world"),
+        #         "ns"     : LaunchConfiguration("ns"),
+        #         "gviz"   : LaunchConfiguration("gviz"),
+        #         "rviz"   : LaunchConfiguration("rviz"),
+        #         "sviz"   : LaunchConfiguration("sviz"),
+        #         "output" : LaunchConfiguration("output"), 
+        #         "drivers": "true",                          # Now start driver nodes
+        #         "spurn"  : LaunchConfiguration("spurn"),    # Prevent node
+        #         "nodes"  : LaunchConfiguration("nodes"),    # Launch node group
+        #         "extra"  : LaunchConfiguration("extra"),    # Inject extra nodes
+        #         "llp"    : LaunchConfiguration("llp"),
+        #         "mlp"    : LaunchConfiguration("mlp"),
+        #         "dds"    : LaunchConfiguration("dds"),
+        #     }.items(),
+        # ),
+    ])
diff --git a/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h b/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
index 769abdea5e..aafc531d75 100644
--- a/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
+++ b/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
@@ -39,7 +39,7 @@ namespace traj_opt_msgs {
   typedef msg::Spline Spline;
   typedef msg::Trajectory Trajectory;
   typedef msg::SolverInfo SolverInfo;
-} // namespace traj_opt_msgs
+}  // namespace traj_opt_msgs
 
 
 class TrajRosBridge {
@@ -78,7 +78,6 @@ class TrajRosBridge {
   NodeHandle nh_info_;
   std::map<std::string, rclcpp::Publisher<traj_opt_msgs::Trajectory>::SharedPtr> pubs_traj_;
   std::map<std::string, rclcpp::Publisher<traj_opt_msgs::SolverInfo>::SharedPtr> pubs_info_;
-
 };
 
-#endif  // TRAJ_OPT_ROS_ROS_BRIDGE_H_
+#endif  // TRAJ_OPT_ROS_ROS_BRIDGE_H_
\ No newline at end of file
diff --git a/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp b/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
index bed14a0d51..5ef5c3fc5e 100644
--- a/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
+++ b/mobility/planner_qp/traj_opt_ros/src/ros_bridge.cpp
@@ -20,9 +20,9 @@
 #include <ff_util/ff_component.h>
 #include <string>
 
-TrajRosBridge::TrajRosBridge(){
-  nh_traj_ = std::make_shared<rclcpp::Node>("~","~");
-  nh_info_ = std::make_shared<rclcpp::Node>("~","~");
+TrajRosBridge::TrajRosBridge() {
+  nh_traj_ = std::make_shared<rclcpp::Node>("~", "~");
+  nh_info_ = std::make_shared<rclcpp::Node>("~", "~");
   // rclcpp::Node n("~");
   // nh_ = std::shared_ptr(n);
 }

From 2fb68f445b5c8ac76759f2c5e8ef3c026fa1bb00 Mon Sep 17 00:00:00 2001
From: joris997 <joris.verhagen@gmail.com>
Date: Wed, 31 May 2023 14:15:46 +0200
Subject: [PATCH 8/9] changed accidental file addition of granite.py launch
 file

---
 astrobee/launch/granite.launch.py | 210 ------------------------------
 1 file changed, 210 deletions(-)
 delete mode 100644 astrobee/launch/granite.launch.py

diff --git a/astrobee/launch/granite.launch.py b/astrobee/launch/granite.launch.py
deleted file mode 100644
index 8734cdec9c..0000000000
--- a/astrobee/launch/granite.launch.py
+++ /dev/null
@@ -1,210 +0,0 @@
-# Copyright (c) 2017, United States Government, as represented by the
-# Administrator of the National Aeronautics and Space Administration.
-#
-# All rights reserved.
-#
-# The Astrobee platform is licensed under the Apache License, Version 2.0
-# (the "License"); you may not use this file except in compliance with the
-# License. You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
-# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
-# License for the specific language governing permissions and limitations
-# under the License.
-
-
-from utilities.utilities import *
-
-def generate_launch_description():
-    print("asdf")
-    robot_description = Command(['xacro ', get_path('urdf/model.urdf.xacro', 'description'),
-                        ' world:="',      LaunchConfiguration('world'),
-                        '" top_aft:="',   LaunchConfiguration('top_aft'),
-                        '" bot_aft:="',   LaunchConfiguration('bot_aft'),
-                        '" bot_front:="', LaunchConfiguration('bot_front'), 
-                        '" ns:="_',       LaunchConfiguration('ns'),
-                        '" prefix:="',    LaunchConfiguration('ns'), '/"' ])
-    # robot_description = Command(['xacro ', get_path('urdf/model_carriage.urdf.xacro')])
-    print("asdf")
-
-    return LaunchDescription([
-        # Context options (NB: THESE ARE OVERRIDDEN BY ENVIRONMENT VARIABLES)   -->
-        # Set world and world correctly; environment variable over rule default -->
-        DeclareLaunchArgument("robot", default_value=os.getenv("ASTROBEE_ROBOT", "bsharp")),
-        DeclareLaunchArgument("world", default_value=os.getenv("ASTROBEE_WORLD", "granite")),
-
-        DeclareLaunchArgument("ns",     default_value=""),     # Robot namespace
-        DeclareLaunchArgument("spurn",  default_value=""),     # Prevent a specific node
-        DeclareLaunchArgument("nodes",  default_value=""),     # Launch specific nodes
-        DeclareLaunchArgument("extra",  default_value=""),     # Inject an additional node
-        DeclareLaunchArgument("debug",  default_value=""),     # Debug node group
-        DeclareLaunchArgument("dds",    default_value="true"), # Enable DDS
-        
-        # General options
-        DeclareLaunchArgument("vive",   default_value="false"),
-        DeclareLaunchArgument("gviz",   default_value="false"),
-        DeclareLaunchArgument("rviz",   default_value="false"),
-        DeclareLaunchArgument("sviz",   default_value="false"),
-        DeclareLaunchArgument("rqt",    default_value="false"),
-        DeclareLaunchArgument("gds",    default_value="false"),
-        DeclareLaunchArgument("agent1", default_value="Bsharp"),
-        DeclareLaunchArgument("agent2", default_value="Bumble"),
-        DeclareLaunchArgument("agent3", default_value="Honey"),
-        DeclareLaunchArgument("stats",  default_value="false"),
-
-        # Remaining options
-        DeclareLaunchArgument("output",  default_value="log"),    # Output to screen or log
-        DeclareLaunchArgument("gtloc",   default_value="false"),  # Use Ground Truth Localizer
-        DeclareLaunchArgument("drivers", default_value="true"),   # Should we launch drivers?
-        DeclareLaunchArgument("llp",     default_value="10.42.0.37"),  # LLP IP address
-        DeclareLaunchArgument("mlp",     default_value="10.42.0.38"),  # MLP IP address
-        DeclareLaunchArgument("rec",     default_value=""),
-        DeclareLaunchArgument("vmware",  default_value="true"),
-        DeclareLaunchArgument("speed",   default_value="1"),
-        DeclareLaunchArgument("physics", default_value="ode"),
-        DeclareLaunchArgument("sim",     default_value="local"),
-
-
-        # Simulation options only -->
-        DeclareLaunchArgument("pose", default_value="0 0 -0.15 0 0 0"),  # Initial pose (sim only)
-
-        # Path to the bag file
-        DeclareLaunchArgument("bag", default_value=""),
-
-        # Make sure all environment variables are set for controller
-        # Override the robot and world environment variables all the time. The
-        # environment variables are the default if they are set. So in this
-        # case we are overriding the environment variables with themselves.
-        # Roslaunch arguments override the environment variable which is what
-        # this will do.
-        SetEnvironmentVariable(
-            name='ASTROBEE_ROBOT',
-            value=LaunchConfiguration('robot'),
-        ),
-        SetEnvironmentVariable(
-            name='ASTROBEE_WORLD',
-            value=LaunchConfiguration('world'),
-        ),
-        SetEnvironmentVariable(
-            name='ASTROBEE_CONFIG_DIR',
-            value=os.getenv("ASTROBEE_CONFIG_DIR",get_path("config")),
-        ),
-        SetEnvironmentVariable(
-            name='ASTROBEE_RESOURCE_DIR',
-            value=os.getenv("ASTROBEE_RESOURCE_DIR",get_path("resources")),
-        ),
-        SetEnvironmentVariable(
-            name='ROSCONSOLE_CONFIG_FILE',
-            value=os.getenv("ROSCONSOLE_CONFIG_FILE",get_path("resources/logging.config")),
-        ),
-
-        SetEnvironmentVariable(
-            name='ROSCONSOLE_FORMAT',
-            value="[${severity}] [${time}] : (${logger}) ${message}"
-        ),
-
-        # Start ground controller services
-        IncludeLaunchDescription(
-            get_launch_file("launch/controller/descriptions.launch.py"),
-            launch_arguments={
-                "world": LaunchConfiguration("world"),
-            }.items(),
-        ),
-        IncludeLaunchDescription(
-            get_launch_file("launch/controller/stats.launch.py"),
-            condition=IfCondition(LaunchConfiguration("stats")),
-        ),
-        IncludeLaunchDescription(
-            get_launch_file("launch/controller/vive.launch.py"),
-            condition=IfCondition(LaunchConfiguration("vive")),
-        ),
-        IncludeLaunchDescription(
-            get_launch_file("launch/controller/gviz.launch.py"),
-            condition=IfCondition(LaunchConfiguration("gviz")),
-        ),
-        IncludeLaunchDescription(
-            get_launch_file("launch/controller/gds.launch.py"),
-            launch_arguments={
-                "world" : LaunchConfiguration("world"),
-                "agent1": LaunchConfiguration("agent1"),
-                "agent2": LaunchConfiguration("agent2"),
-                "agent3": LaunchConfiguration("agent3"),
-            }.items(),
-            condition=IfCondition(LaunchConfiguration("gds")),
-        ),
-        IncludeLaunchDescription(
-            get_launch_file("launch/controller/rqt.launch.py"),
-            condition=IfCondition(LaunchConfiguration("rqt")),
-        ),
-        IncludeLaunchDescription(
-            get_launch_file("launch/controller/rviz.launch.py"),
-            launch_arguments={
-                "world" : LaunchConfiguration("world"),
-            }.items(),
-            condition=IfCondition(LaunchConfiguration("rviz")),
-        ),
-        # Launch a recorder for this robot
-        IncludeLaunchDescription(
-            get_launch_file("launch/controller/bagrecord.launch.py"),
-            condition=LaunchConfigurationNotEquals("rec", ""),
-            launch_arguments={"bag": LaunchConfiguration("rec")}.items(),
-        ),
-
-        # Start the simulator
-        IncludeLaunchDescription(
-            get_launch_file("launch/controller/sim_start.launch.py"),
-            launch_arguments={
-                "world"  : LaunchConfiguration("world"),
-                "sviz"   : LaunchConfiguration("sviz"),
-                "vmware" : LaunchConfiguration("vmware"),
-                "speed"  : LaunchConfiguration("speed"),
-                "debug"  : LaunchConfiguration("debug"),
-                "physics": LaunchConfiguration("physics"),
-            }.items(),
-        ),
-
-        # # Auto-inert platform #1 at a desired initial location
-        # IncludeLaunchDescription(
-        #     get_launch_file("launch/spawn.launch.py"),
-        #     launch_arguments={
-        #         "robot" : LaunchConfiguration("robot"),      # Type of robot
-        #         "world" : LaunchConfiguration("world"),      # Execution context
-        #         "ns"    : LaunchConfiguration("ns"),         # Robot namespace
-        #         "output": LaunchConfiguration("output"),     # Output for logging
-        #         "pose"  : LaunchConfiguration("pose"),       # Initial robot pose
-        #         "spurn" : LaunchConfiguration("spurn"),      # Prevent node
-        #         "nodes" : LaunchConfiguration("nodes"),      # Launch node group
-        #         "extra" : LaunchConfiguration("extra"),      # Inject extra nodes
-        #         "debug" : LaunchConfiguration("debug"),      # Debug a node set
-        #         "sim"   : LaunchConfiguration("sim"),        # SIM IP address
-        #         "llp"   : LaunchConfiguration("llp"),        # LLP IP address
-        #         "mlp"   : LaunchConfiguration("mlp"),        # MLP IP address
-        #         "dds"   : LaunchConfiguration("dds"),        # Enable DDS
-        #         "gtloc" : LaunchConfiguration("gtloc"),      # Use Ground Truth Localizer
-        #     }.items(),
-        # ),
-
-        # # bsharp
-        # IncludeLaunchDescription(
-        #     get_launch_file("launch/astrobee.launch.py"),
-        #     launch_arguments={
-        #         "robot"  : LaunchConfiguration("robot"),
-        #         "world"  : LaunchConfiguration("world"),
-        #         "ns"     : LaunchConfiguration("ns"),
-        #         "gviz"   : LaunchConfiguration("gviz"),
-        #         "rviz"   : LaunchConfiguration("rviz"),
-        #         "sviz"   : LaunchConfiguration("sviz"),
-        #         "output" : LaunchConfiguration("output"), 
-        #         "drivers": "true",                          # Now start driver nodes
-        #         "spurn"  : LaunchConfiguration("spurn"),    # Prevent node
-        #         "nodes"  : LaunchConfiguration("nodes"),    # Launch node group
-        #         "extra"  : LaunchConfiguration("extra"),    # Inject extra nodes
-        #         "llp"    : LaunchConfiguration("llp"),
-        #         "mlp"    : LaunchConfiguration("mlp"),
-        #         "dds"    : LaunchConfiguration("dds"),
-        #     }.items(),
-        # ),
-    ])

From cd5a91452a8da54619275744475449f719b72322 Mon Sep 17 00:00:00 2001
From: joris997 <joris.verhagen@gmail.com>
Date: Wed, 31 May 2023 14:19:53 +0200
Subject: [PATCH 9/9] another lint error

---
 .../planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h   | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h b/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
index aafc531d75..30ad96690d 100644
--- a/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
+++ b/mobility/planner_qp/traj_opt_ros/include/traj_opt_ros/ros_bridge.h
@@ -80,4 +80,4 @@ class TrajRosBridge {
   std::map<std::string, rclcpp::Publisher<traj_opt_msgs::SolverInfo>::SharedPtr> pubs_info_;
 };
 
-#endif  // TRAJ_OPT_ROS_ROS_BRIDGE_H_
\ No newline at end of file
+#endif  // TRAJ_OPT_ROS_ROS_BRIDGE_H_