From d634307c8c5ea107f6293046da0749e653af5e3d Mon Sep 17 00:00:00 2001
From: Matthias Holoch <mholoch@gmail.com>
Date: Thu, 25 Jan 2024 17:22:38 +0100
Subject: [PATCH] use float msg for debugging from example_interfaces

std_msgs float got removed after foxy
---
 mesh_controller/CMakeLists.txt                            | 6 ++++--
 mesh_controller/include/mesh_controller/mesh_controller.h | 3 ++-
 mesh_controller/package.xml                               | 1 +
 mesh_controller/src/mesh_controller.cpp                   | 5 ++---
 4 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/mesh_controller/CMakeLists.txt b/mesh_controller/CMakeLists.txt
index f9778ffb..28f7b4e9 100644
--- a/mesh_controller/CMakeLists.txt
+++ b/mesh_controller/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.8)
 project(mesh_controller)
 
 find_package(ament_cmake_ros REQUIRED)
+find_package(example_interfaces REQUIRED)
 find_package(mbf_mesh_core REQUIRED)
 find_package(mbf_msgs REQUIRED)
 find_package(mbf_utility REQUIRED)
@@ -9,6 +10,7 @@ find_package(mesh_map REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 
+
 pluginlib_export_plugin_description_file(mbf_mesh_core mesh_controller.xml)
 
 add_library(${PROJECT_NAME} src/mesh_controller.cpp)
@@ -16,7 +18,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 target_compile_definitions(${PROJECT_NAME} PRIVATE "MESH_CONTROLLER_BUILDING_LIBRARY")
-ament_target_dependencies(${PROJECT_NAME} mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp tf2_geometry_msgs)
+ament_target_dependencies(${PROJECT_NAME} example_interfaces mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp tf2_geometry_msgs)
 
 install(DIRECTORY include DESTINATION include)
 install(TARGETS ${PROJECT_NAME}
@@ -29,5 +31,5 @@ install(TARGETS ${PROJECT_NAME}
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 ament_export_targets(export_${PROJECT_NAME})
-ament_export_dependencies(mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp tf2_geometry_msgs)
+ament_export_dependencies(example_interfaces mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp tf2_geometry_msgs)
 ament_package()
\ No newline at end of file
diff --git a/mesh_controller/include/mesh_controller/mesh_controller.h b/mesh_controller/include/mesh_controller/mesh_controller.h
index 8fe98338..3796b03e 100644
--- a/mesh_controller/include/mesh_controller/mesh_controller.h
+++ b/mesh_controller/include/mesh_controller/mesh_controller.h
@@ -37,6 +37,7 @@
 
 #include <mbf_mesh_core/mesh_controller.h>
 #include <mbf_msgs/action/get_path.hpp>
+#include <example_interfaces/msg/float32.hpp>
 #include <mesh_map/mesh_map.h>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -180,7 +181,7 @@ class MeshController : public mbf_mesh_core::MeshController
   lvr2::DenseVertexMap<mesh_map::Vector> vector_map_;
 
   //! publishes the angle between the robots orientation and the goal vector field for debug purposes
-  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr angle_pub_;
+  rclcpp::Publisher<example_interfaces::msg::Float32>::SharedPtr angle_pub_;
 
   //! flag to handle cancel requests
   std::atomic_bool cancel_requested_;
diff --git a/mesh_controller/package.xml b/mesh_controller/package.xml
index 51f321c6..35c8aef2 100644
--- a/mesh_controller/package.xml
+++ b/mesh_controller/package.xml
@@ -15,6 +15,7 @@
     <depend>mesh_map</depend>
     <depend>rclcpp</depend>
     <depend>tf2_geometry_msgs</depend>
+    <depend>example_interfaces</depend>
 
     <export>
         <build_type>ament_cmake</build_type>
diff --git a/mesh_controller/src/mesh_controller.cpp b/mesh_controller/src/mesh_controller.cpp
index 7abfbd70..2e186ac3 100644
--- a/mesh_controller/src/mesh_controller.cpp
+++ b/mesh_controller/src/mesh_controller.cpp
@@ -41,7 +41,6 @@
 #include <mesh_controller/mesh_controller.h>
 #include <mesh_map/util.h>
 #include <pluginlib/class_list_macros.hpp>
-#include <std_msgs/msg/float32.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <mbf_utility/exe_path_exception.h>
 
@@ -229,7 +228,7 @@ std::array<float, 2> MeshController::naiveControl(
   float phi = acos(mesh_dir.dot(robot_dir));
   float sign_phi = mesh_dir.cross(robot_dir).dot(mesh_normal);
   // debug output angle between supposed and current angle
-  DEBUG_CALL(std_msgs::msg::Float32 angle32; angle32.data = phi * 180 / M_PI; angle_pub_->publish(angle32);)
+  DEBUG_CALL(example_interfaces::msg::Float32 angle32; angle32.data = phi * 180 / M_PI; angle_pub_->publish(angle32);)
 
   float angular_velocity = copysignf(phi * config_.max_ang_velocity / M_PI, -sign_phi);
   const float max_angle = config_.max_angle * M_PI / 180.0;
@@ -275,7 +274,7 @@ bool MeshController::initialize(const std::string& plugin_name,
   map_ptr_ = mesh_map_ptr;
   name_ = plugin_name;
 
-  angle_pub_ = node_->create_publisher<std_msgs::msg::Float32>("~/current_angle", rclcpp::QoS(1).transient_local());
+  angle_pub_ = node_->create_publisher<example_interfaces::msg::Float32>("~/current_angle", rclcpp::QoS(1).transient_local());
 
   { // cost max_lin_velocity
     rcl_interfaces::msg::ParameterDescriptor descriptor;