Clean droan (#32)

* Attempt to clean up

* Add config and launch for droan

* fixed parameter exception and set map representation to disaprity

* fixed pluginlib not finding disparity map representation

* Clear ament_cmake_prefix and cmake_prefix on cws

* Rename core_map_representation_interface --> map_representation_interface

* Add unused mav_msgs dependency

* removed virtual destructor

* Remove old commet

* Rename .h to .hpp

---------

Co-authored-by: John Keller <[email protected]>

docker/.bashrc

@@ -141,6 +141,8 @@ function cws(){
         echo "Cleaning ROS2 workspace..."
         set -x
         rm -rf "$ROS2_WS_DIR"/build/ "$ROS2_WS_DIR"/install/ "$ROS2_WS_DIR"/log/
+        export AMENT_PREFIX_PATH="/opt/ros/humble"
+        export CMAKE_PREFIX_PATH=""
         { set +x; } 2>/dev/null  # set +x w/out it being printed
         echo "ROS2 workspace has been cleaned."
     else

ros_ws/src/airstack_common/include/airstack_common/ros2_helper.hpp

@@ -16,7 +16,10 @@ namespace airstack {
 
   template <>
   inline int get_param(rclcpp::Node* node, std::string name, int default_value, bool* set){
-    node->declare_parameter(name, rclcpp::PARAMETER_INTEGER);
+    try{
+      node->declare_parameter(name, rclcpp::PARAMETER_INTEGER);
+    }
+    catch(rclcpp::exceptions::ParameterAlreadyDeclaredException& e){}
     rclcpp::Parameter param;
     bool s = node->get_parameter_or(name, param, rclcpp::Parameter(name, default_value));
     if(set != NULL)
@@ -26,7 +29,10 @@ namespace airstack {
 
   template <>
   inline double get_param(rclcpp::Node* node, std::string name, double default_value, bool* set){
-    node->declare_parameter(name, rclcpp::PARAMETER_DOUBLE);
+    try{
+      node->declare_parameter(name, rclcpp::PARAMETER_DOUBLE);
+    }
+    catch(rclcpp::exceptions::ParameterAlreadyDeclaredException& e){}
     rclcpp::Parameter param;
     bool s = node->get_parameter_or(name, param, rclcpp::Parameter(name, default_value));
     if(set != NULL)
@@ -36,7 +42,10 @@ namespace airstack {
 
   template <>
   inline std::string get_param(rclcpp::Node* node, std::string name, std::string default_value, bool* set){
-    node->declare_parameter(name, rclcpp::PARAMETER_STRING);
+    try{
+      node->declare_parameter(name, rclcpp::PARAMETER_STRING);
+    }
+    catch(rclcpp::exceptions::ParameterAlreadyDeclaredException& e){}
     rclcpp::Parameter param;
     bool s = node->get_parameter_or(name, param, rclcpp::Parameter(name, default_value));
     if(set != NULL)
@@ -46,7 +55,10 @@ namespace airstack {
 
   template <>
   inline bool get_param(rclcpp::Node* node, std::string name, bool default_value, bool* set){
-    node->declare_parameter(name, rclcpp::PARAMETER_BOOL);
+    try{
+      node->declare_parameter(name, rclcpp::PARAMETER_BOOL);
+    }
+    catch(rclcpp::exceptions::ParameterAlreadyDeclaredException& e){}
     rclcpp::Parameter param;
     bool s = node->get_parameter_or(name, param, rclcpp::Parameter(name, default_value));
     if(set != NULL)

ros_ws/src/robot/autonomy/planning/local/disparity_graph/include/disparity_graph/disparity_graph.hpp

@@ -115,7 +115,7 @@ class DisparityGraph : rclcpp::Node {
             std::bind(&DisparityGraph::get_cam_info, this, std::placeholders::_1));
     }
 
-    virtual ~DisparityGraph();
+  //virtual ~DisparityGraph();
 
     void disp_cb(const sensor_msgs::msg::Image::ConstSharedPtr &disp_fg,
                  const sensor_msgs::msg::Image::ConstSharedPtr &disp_bg) {

ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/CMakeLists.txt

@@ -1,11 +1,16 @@
 cmake_minimum_required(VERSION 3.5)
 project(disparity_map_representation)
 
-find_package(ament_cmake REQUIRED)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
 
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
 find_package(airstack_common REQUIRED)
 find_package(airstack_msgs REQUIRED)
-find_package(core_map_representation_interface REQUIRED)
+find_package(map_representation_interface REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(disparity_graph REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -25,76 +30,78 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(visualization_msgs REQUIRED)
 
-ament_export_include_directories(include)
-ament_export_libraries(disparity_graph)
-
-include_directories(
-  ${ament_INCLUDE_DIRS}
-  ${CMAKE_CURRENT_SOURCE_DIR}/include
-  ${core_map_representation_interface_INCLUDE_DIRS}
-  ${disparity_graph_INCLUDE_DIRS}
-  ${pluginlib_INCLUDE_DIRS}
 
-  # ${CMAKE_CURRENT_SOURCE_DIR}/include
-)
-
-add_library(disparity_map_representation
-  src/disparity_map_representation.cpp
-)
+add_library(disparity_map_representation src/disparity_map_representation.cpp)
+target_compile_features(disparity_map_representation PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
+target_include_directories(disparity_map_representation PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
 
-target_link_libraries(disparity_map_representation
-  disparity_graph
-  ${pluginlib_LIBRARIES} # Ensure this variable is set correctly
-)
-
-ament_target_dependencies(disparity_map_representation
+ament_target_dependencies(
+  disparity_map_representation
   airstack_common
   airstack_msgs
-  core_map_representation_interface
+  map_representation_interface
   cv_bridge
   disparity_graph
+  geometry_msgs
+  image_geometry
+  image_transport
+  nav_msgs
+  pcl_msgs
   pluginlib
   rclcpp
+  rclpy
+  sensor_msgs
+  std_msgs
+  stereo_msgs
   tf2
+  tf2_eigen
+  tf2_geometry_msgs
   tf2_ros
-)
+  visualization_msgs
+  )
 
-ament_export_targets(disparity_map_representationTargets HAS_LIBRARY_TARGET)
-ament_export_dependencies(rclcpp
-  airstack_common
-  airstack_msgs
-  core_map_representation_interface
-  cv_bridge
-  disparity_graph
-  pluginlib
-  tf2
-  tf2_ros
-)
+pluginlib_export_plugin_description_file(map_representation_interface disparity_map_representation_plugin.xml)
 
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+#target_compile_definitions(disparity_map_representation PRIVATE "disparity_map_representation_INTERFACE_BUILDING_LIBRARY")
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
 install(
   TARGETS disparity_map_representation
-  EXPORT disparity_map_representationTargets
-  LIBRARY DESTINATION lib
+  EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
-  INCLUDES DESTINATION include
 )
 
-# install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
-install(FILES package.xml
-  DESTINATION share/${PROJECT_NAME}
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(
+  include
 )
-target_link_libraries(disparity_map_representation
-  ${disparity_graph_LIBRARIES}
+ament_export_libraries(
+  disparity_map_representation
 )
-
-# ament_export_include_directories(include)
-# ament_export_libraries(disparity_map_representation)
-# ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-install(
-  DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION include/${PROJECT_NAME}
-  FILES_MATCHING PATTERN "*.h"
+ament_export_targets(
+  export_${PROJECT_NAME}
 )
 
 ament_package()

ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/disparity_map_representation_plugin.xml

@@ -1,5 +1,6 @@
-<library path="lib/libdisparity_map_representation">
-  <class type="DisparityMapRepresentation" base_class_type="MapRepresentation">
+<library path="disparity_map_representation">
+  <class type="disparity_map_representation::DisparityMapRepresentation"
+    base_class_type="map_representation_interface::MapRepresentation">
     <description>Disparity map representation plugin.</description>
   </class>
-</library>
+</library>

ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp

@@ -1,6 +1,5 @@
 #ifndef _DISPARITY_MAP_REPRESENTATION_
 #define _DISPARITY_MAP_REPRESENTATION_
-#include <core_map_representation_interface/map_representation.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/time_synchronizer.h>
 #include <tf2/LinearMath/Transform.h>
@@ -9,6 +8,7 @@
 #include <airstack_msgs/msg/trajectory_xyzv_yaw.hpp>
 #include <disparity_graph/disparity_graph.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <map_representation_interface/map_representation.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
@@ -18,7 +18,8 @@
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
-class DisparityMapRepresentation : public MapRepresentation {
+namespace disparity_map_representation {
+class DisparityMapRepresentation : public map_representation_interface::MapRepresentation {
    private:
     std::unique_ptr<disparity_graph::DisparityGraph> disp_graph;
 
@@ -45,5 +46,6 @@ class DisparityMapRepresentation : public MapRepresentation {
     virtual std::vector<std::vector<double> > get_values(
         std::vector<std::vector<geometry_msgs::msg::PointStamped> > trajectories);
 };
+}  // namespace disparity_map_representation
 
 #endif

ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/package.xml

@@ -12,7 +12,7 @@
 
   <depend>airstack_common</depend>
   <depend>airstack_msgs</depend>
-  <depend>core_map_representation_interface</depend>
+  <depend>map_representation_interface</depend>
   <depend>cv_bridge</depend>
   <depend>disparity_graph</depend>
   <depend>geometry_msgs</depend>
@@ -32,6 +32,5 @@
 
   <export>
     <build_type>ament_cmake</build_type>
-    <core_map_representation_interface plugin="${prefix}/disparity_map_representation_plugin.xml" />
   </export>
-</package>
+</package>

ros_ws/src/robot/autonomy/planning/local/disparity_map_representation/src/disparity_map_representation.cpp

@@ -1,6 +1,5 @@
 #include <disparity_map_representation/disparity_map_representation.hpp>
-#include <pluginlib/class_list_macros.hpp>
-
+namespace disparity_map_representation {
 DisparityMapRepresentation::DisparityMapRepresentation()
     : MapRepresentation(), disp_graph(std::make_unique<disparity_graph::DisparityGraph>()) {
     points.ns = "obstacles";
@@ -256,5 +255,8 @@ void DisparityMapRepresentation::publish_debug() {
     points.points.clear();
     points.colors.clear();
 }
+}  // namespace disparity_map_representation
+#include <pluginlib/class_list_macros.hpp>
 
-PLUGINLIB_EXPORT_CLASS(DisparityMapRepresentation, MapRepresentation)
+PLUGINLIB_EXPORT_CLASS(disparity_map_representation::DisparityMapRepresentation,
+                       map_representation_interface::MapRepresentation)

ros_ws/src/robot/autonomy/planning/local/droan_local_planner/CMakeLists.txt

@@ -10,7 +10,7 @@ find_package(ament_cmake REQUIRED)
 
 find_package(airstack_common REQUIRED)
 find_package(airstack_msgs REQUIRED)
-find_package(core_map_representation_interface REQUIRED)
+find_package(map_representation_interface REQUIRED)
 # find_package(disparity_map_representation REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -29,7 +29,7 @@ ament_target_dependencies(
   droan_local_planner
   "airstack_msgs"
   "airstack_common"
-  "core_map_representation_interface"
+  "map_representation_interface"
   # "disparity_map_representation"
   "trajectory_controller"
   "trajectory_library"