f/690 - Fixes

Author: WaspishRaccoon

src/wika_pressure_sensor/CMakeLists.txt src/pressure_sensor/CMakeLists.txtsrc/wika_pressure_sensor/CMakeLists.txt renamed to src/pressure_sensor/CMakeLists.txt

@@ -1,42 +1,36 @@
 cmake_minimum_required(VERSION 3.8)
-project(wika_pressure_sensor)
+project(pressure_sensor)
 
 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(rclcpp REQUIRED)
-find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 
-include_directories(
-  include/
-)
+include_directories(include)
 
-add_executable(wika_sensor_node 
-  src/wika_sensor_node.cpp
+add_executable(${PROJECT_NAME} 
+  src/pressure_sensor_impl.cpp
+  src/pressure_sensor.cpp
 )
 
-ament_target_dependencies(wika_sensor_node 
-  rclcpp 
-  sensor_msgs 
+ament_target_dependencies(${PROJECT_NAME} 
+  rclcpp
   std_msgs
 )
 
-install(TARGETS
-  wika_sensor_node
+install(TARGETS ${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME}
 )
 
-install(
-  DIRECTORY include/
+install(DIRECTORY include/
   DESTINATION include/
 )
 
-install(
-  DIRECTORY launch/
-  DESTINATION share/${PROJECT_NAME}/launch
+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/
 )
 
 if(BUILD_TESTING)

src/pressure_sensor/config/pressure_sensor_params.yaml

@@ -0,0 +1,3 @@
+pressure_sensor_impl:
+  ros__parameters:
+    dump_param: 1.0 # FIXME remove in the future

src/pressure_sensor/include/pressure_sensor/pressure_sensor_impl.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#define DEFAULT_DUMP_PARAM 1.0
+
+namespace stingray_core::pressure_sensor {
+
+class PressureSensorNode {
+   public:
+    explicit PressureSensorNode(
+        rclcpp::NodeOptions options = rclcpp::NodeOptions());
+
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+    get_node_base_interface() {
+        return node_->get_node_base_interface();
+    }
+
+    rclcpp::Logger get_logger() const { return node_->get_logger(); }
+
+   private:
+    void data_raw_callback(const std_msgs::msg::String::ConstSharedPtr& msg);
+    void publish_depth(const double depth);
+
+    std::shared_ptr<rclcpp::Node> node_;
+
+    double dump_param_;
+
+    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr depth_pub_;
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr data_raw_sub_;
+};
+
+}  // namespace stingray_core::pressure_sensor

src/pressure_sensor/launch/pressure_sensor.launch.py

@@ -0,0 +1,27 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+import os
+
+PRESSURE_SENSOR_PACKAGE = 'pressure_sensor'
+PRESSURE_SENSOR_NODE_NAME = 'pressure_sensor_impl'
+PRESSURE_SENSOR_NAMESPACE = 'stingray_core/pressure_sensor'
+
+
+def generate_launch_description():
+    package_dir = get_package_share_directory(PRESSURE_SENSOR_PACKAGE)
+    config_file = os.path.join(
+        package_dir, 'config', 'pressure_sensor_params.yaml')
+
+    return LaunchDescription([
+        Node(
+            package=PRESSURE_SENSOR_PACKAGE,
+            executable=PRESSURE_SENSOR_NODE_NAME,
+            name=PRESSURE_SENSOR_NODE_NAME,
+            output='screen',
+            namespace=PRESSURE_SENSOR_NAMESPACE,
+            parameters=[config_file],
+            emulate_tty=True,
+        )
+    ])

src/wika_pressure_sensor/package.xml src/pressure_sensor/package.xmlsrc/wika_pressure_sensor/package.xml renamed to src/pressure_sensor/package.xml

@@ -1,16 +1,14 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>wika_pressure_sensor</name>
+  <name>pressure_sensor</name>
   <version>1.0.0</version>
-  <description>WIKA pressure sensor data parser node</description>
-  <maintainer email="p.v.elesin@gmail.com">waspishraccoon</maintainer>
-  <license>TODO: License declaration</license>
+  <description>Pressure sensor data parser node</description>
+  <maintainer email="p.v.elesin@gmail.com">Pavel Elesin</maintainer>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
-  <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
 
   <exec_depend>launch</exec_depend>

src/pressure_sensor/src/pressure_sensor.cpp

@@ -0,0 +1,11 @@
+#include <pressure_sensor/pressure_sensor_node.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+int main(int argc, char** argv) {
+    rclcpp::init(argc, argv);
+    auto sensor_node =
+        std::make_shared<stingray_core::pressure_sensor::PressureSensorNode>();
+    rclcpp::spin(sensor_node->get_node_base_interface());
+    rclcpp::shutdown();
+    return 0;
+}

src/pressure_sensor/src/pressure_sensor_impl.cpp

@@ -0,0 +1,35 @@
+#include <pressure_sensor/pressure_sensor_impl.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace stingray_core::pressure_sensor {
+
+PressureSensorNode::PressureSensorNode(rclcpp::NodeOptions options)
+    : node_(rclcpp::Node::make_shared("pressure_sensor_impl",
+                                      std::move(options))),
+      depth_pub_(node_->create_publisher<std_msgs::msg::Float64>("depth", 10)),
+      data_raw_sub_(node_->create_subscription<std_msgs::msg::String>(
+          "/stingray_core/depth_link_node/data_raw", 10,
+          [this](const std_msgs::msg::String::SharedPtr& msg) {
+              this->data_raw_callback(msg)
+          })) {
+    node_->declare_parameter<double>("dump_param", DEFAULT_DUMP_PARAM);
+
+    dump_param_ = node_->get_parameter("dump_param").as_double();
+
+    RCLCPP_INFO(node_->get_logger(), "Pressure sensor node initialized");
+    RCLCPP_INFO(node_->get_logger(), "dump_param: %.3f", dump_param_);
+}
+
+void PressureSensorNode::data_raw_callback(
+    const std_msgs::msg::String::ConstSharedPtr& msg) {
+    const double depth = std::stod(msg->data) * dump_param_;
+    publish_depth(depth);
+}
+
+void PressureSensorNode::publish_depth(const double depth) {
+    const auto depth_msg = std_msgs::msg::Float64::build().data(depth);
+    depth_pub_->publish(depth_msg);
+    RCLCPP_DEBUG(node_->get_logger(), "Published depth: %.3f m", depth);
+}
+
+}  // namespace stingray_core::pressure_sensor