From 37f366e4087e3476fa254656ece52f346daa413d Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 5 Apr 2022 16:43:50 +0200
Subject: [PATCH] Added the messages and services definitions needed by the
 opendrive global planner.

---
 CMakeLists.txt              | 45 +++++++++++++++++++++++++------------
 msg/adc_opendrive_nodes.msg |  2 ++
 srv/get_opendrive_map.srv   |  2 ++
 srv/get_opendrive_nodes.srv |  2 ++
 4 files changed, 37 insertions(+), 14 deletions(-)
 create mode 100644 msg/adc_opendrive_nodes.msg
 create mode 100644 srv/get_opendrive_map.srv
 create mode 100644 srv/get_opendrive_nodes.srv

diff --git a/CMakeLists.txt b/CMakeLists.txt
index a3d2b20..9c0e500 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,7 @@ add_definitions(-std=c++11)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf2_ros nav_msgs iri_base_algorithm iri_adc_msgs costmap_2d)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf2_ros nav_msgs iri_base_algorithm costmap_2d message_generation iri_behaviortree)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -44,18 +44,21 @@ catkin_python_setup()
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
+ add_message_files(
+   FILES
+   adc_opendrive_nodes.msg
 #   Message1.msg
 #   Message2.msg
-# )
+ )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
+ add_service_files(
+   FILES
+   get_opendrive_map.srv
+   get_opendrive_nodes.srv
 #   Service1.srv
 #   Service2.srv
-# )
+ )
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -65,10 +68,10 @@ catkin_python_setup()
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  nav_msgs std_msgs  # Or other packages containing msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -101,8 +104,8 @@ set(module_name iri_nav_module)
 
 catkin_package(
  INCLUDE_DIRS include
- LIBRARIES ${module_name}
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf2_ros nav_msgs iri_base_algorithm iri_adc_msgs costmap_2d
+ LIBRARIES ${module_name} ${module_name}_bt
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf2_ros nav_msgs iri_base_algorithm costmap_2d message_generation iri_behaviortree
 #  DEPENDS system_lib
 )
 
@@ -116,7 +119,7 @@ include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
 include_directories(${iriutils_INCLUDE_DIR})
 
-## Declare a C++ libraryi
+## Declare a C++ library
 # Module
 add_library(${module_name}
   include/${PROJECT_NAME}/ackermann_lp_module.h 
@@ -124,11 +127,25 @@ add_library(${module_name}
   include/${PROJECT_NAME}/nav_module.h 
   include/${PROJECT_NAME}/nav_planner_module.h 
   include/${PROJECT_NAME}/opendrive_gp_module.h 
+  include/${PROJECT_NAME}/sbpl_gp_module.h 
 )
 set_target_properties(${module_name} PROPERTIES LINKER_LANGUAGE CXX)
 
 target_link_libraries(${module_name} ${catkin_LIBRARIES})
 target_link_libraries(${module_name} ${iriutils_LIBRARY})
+
+set(module_bt_name ${module_name}_bt)
+add_library(${module_bt_name} 
+  include/${PROJECT_NAME}/nav_module_bt.h
+  include/${PROJECT_NAME}/nav_costmap_module_bt.h
+  include/${PROJECT_NAME}/ackermann_lp_module_bt.h)
+set_target_properties(${module_bt_name} PROPERTIES LINKER_LANGUAGE CXX)
+
+target_link_libraries(${module_bt_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_name}.so)
+target_link_libraries(${module_bt_name} ${catkin_LIBRARIES})
+target_link_libraries(${module_bt_name} ${iriutils_LIBRARIES})
+add_dependencies(${module_bt_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${module_name})
+
 ##Link to other modules
 ##target_link_libraries(new_module <module path>/lib<module_name>.so)
 
diff --git a/msg/adc_opendrive_nodes.msg b/msg/adc_opendrive_nodes.msg
new file mode 100644
index 0000000..ae74ef0
--- /dev/null
+++ b/msg/adc_opendrive_nodes.msg
@@ -0,0 +1,2 @@
+Header header
+uint32[] nodes
diff --git a/srv/get_opendrive_map.srv b/srv/get_opendrive_map.srv
new file mode 100644
index 0000000..1105052
--- /dev/null
+++ b/srv/get_opendrive_map.srv
@@ -0,0 +1,2 @@
+---
+nav_msgs/OccupancyGrid opendrive_map
diff --git a/srv/get_opendrive_nodes.srv b/srv/get_opendrive_nodes.srv
new file mode 100644
index 0000000..7849e13
--- /dev/null
+++ b/srv/get_opendrive_nodes.srv
@@ -0,0 +1,2 @@
+---
+iri_nav_module/adc_opendrive_nodes opendrive_nodes
-- 
GitLab