Initial port to C++

clearpath_diagnostics/CMakeLists.txt

@@ -7,14 +7,30 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(ament_cmake_python REQUIRED)
+find_package(clearpath_platform_msgs REQUIRED)
+find_package(diagnostic_updater REQUIRED)
+find_package(rclcpp REQUIRED)
 
-ament_python_install_package(${PROJECT_NAME})
+set(DEPENDENCIES
+  ament_cmake
+  clearpath_platform_msgs
+  diagnostic_updater
+  rclcpp
+)
 
-install(PROGRAMS
-  ${PROJECT_NAME}/diagnostics_updater
-  DESTINATION lib/${PROJECT_NAME}
+add_executable(clearpath_diagnostic_updater
+  src/clearpath_diagnostic_updater.cpp
 )
+target_include_directories(clearpath_diagnostic_updater PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+target_compile_features(clearpath_diagnostic_updater PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+ament_target_dependencies(clearpath_diagnostic_updater ${DEPENDENCIES})
+target_link_libraries(clearpath_diagnostic_updater)
+
+install(TARGETS clearpath_diagnostic_updater
+  DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY config launch
   DESTINATION share/${PROJECT_NAME}

clearpath_diagnostics/config/clearpath_diagnostic_updater.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    serial_number: unknown
+    platform_model: unknown
+    topics:
+      test_topic:
+        type: std_msgs/msg/String
+        rate: 10.0

clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp

@@ -0,0 +1,73 @@
+/**
+Software License Agreement (BSD)
+
+\file      clearpath_diagnostic_updater.hpp
+\authors   Hilary Luo <[email protected]>
+\copyright Copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the
+   following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
+   following disclaimer in the documentation and/or other materials provided with the distribution.
+ * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
+   products derived from this software without specific prior written permission.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
+RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
+DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CLEARPATH_DIAGNOSTIC_UPDATER_HPP
+#define CLEARPATH_DIAGNOSTIC_UPDATER_HPP
+
+#include <map>
+#include <list>
+
+#include "diagnostic_updater/diagnostic_updater.hpp"
+#include "diagnostic_updater/publisher.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+#include "clearpath_platform_msgs/msg/status.hpp"
+
+namespace clearpath
+{
+
+class ClearpathDiagnosticUpdater : public rclcpp::Node
+{
+public:
+  ClearpathDiagnosticUpdater();
+
+private:
+  std::string serial_number_;
+  std::string platform_model_;
+  std::string namespace_;
+
+  // MCU Status Info
+  std::string firmware_version_;
+  int mcu_temperature_;
+  int pcb_temperature_;
+  rclcpp::Subscription<clearpath_platform_msgs::msg::Status>::SharedPtr sub_mcu_status_;
+
+  diagnostic_updater::Updater updater;
+  std::map<std::string, std::map<std::string, rclcpp::Parameter>> topic_map_;
+
+  std::list<double> rates_;
+  std::list<std::shared_ptr<diagnostic_updater::HeaderlessTopicDiagnostic>>
+  headerless_topic_diagnostics_;
+  std::list<std::shared_ptr<rclcpp::GenericSubscription>> subscriptions_;
+
+  void mcuCallback(const clearpath_platform_msgs::msg::Status::SharedPtr msg);
+
+  std::string get_mandatory_param(std::string param_name);
+  void add_topic_rate_diagnostics();
+};
+
+}
+
+#endif  // CLEARPATH_DIAGNOSTIC_UPDATER_HPP