created node to read basics of height sensor

src/beacon_positioning/CMakeLists.txt

@@ -24,14 +24,14 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(positioning_systems_api REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
-find_package(beacon_positioning REQUIRED)
+#find_package(beacon_positioning REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/TrackerPosition.msg" # message for tracker position
 )
 
 add_executable(tracker_position src/tracker_position.cpp)
-ament_target_dependencies(tracker_position rclcpp std_msgs beacon_positioning)
+ament_target_dependencies(tracker_position rclcpp std_msgs)
 
 target_link_libraries(tracker_position
   positioning_systems_api::serial_communication

src/height/CMakeLists.txt

@@ -19,7 +19,6 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(TerabeeApi REQUIRED)
-find_package(height REQUIRED)
 
 add_executable(height_reader src/height_reader.cpp)
 target_include_directories(height_reader PUBLIC
@@ -32,7 +31,7 @@ target_include_directories(height_reader PUBLIC
 ament_target_dependencies(
   height_reader
   rclcpp
-  height
+  TerabeeApi
 )
 
 install(TARGETS height_reader

src/height/package.xml

@@ -3,9 +3,9 @@
 <package format="3">
   <name>height</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>package to read Terabee node</description>
   <maintainer email="semmer99@gmail.com">ubuntu</maintainer>
-  <license>TODO: License declaration</license>
+  <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

src/height/src/height_reader.cpp

@@ -1,10 +1,79 @@
-#include <cstdio>
+#include <chrono>
+#include <iostream>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <terabee/ITerarangerFactory.hpp>
+#include <terabee/ITerarangerEvoMini.hpp>
+#include <terabee/DistanceData.hpp>
+
+using namespace std::chrono_literals;
+
+using terabee::DistanceData;
+using terabee::ITerarangerEvoMini;
+
+std::ostream &operator<<(std::ostream &os, const DistanceData &d)
+{
+  os << "[";
+  for (size_t i = 0; i < d.distance.size(); i++)
+  {
+    os << d.distance[i] << ", ";
+  }
+  os << "\b\b"
+     << " ]";
+  return os;
+}
+
+class HeightReader : public rclcpp::Node
+{
+public:
+  HeightReader() : rclcpp::Node("height_reader")
+  {
+    rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{};
+    descriptor.description = "serial port for the USB port of the height sensor";
+
+    this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor);
+
+    factory = terabee::ITerarangerFactory::getFactory();
+    evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string());
+
+    if (!evo_mini)
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!");
+      return;
+    }
+
+    evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode);
+
+    if (!evo_mini->initialize())
+    {
+      RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!");
+      return;
+    }
+
+    RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!");
+
+    timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this));
+  }
+
+private:
+  void read_height()
+  {
+    std::cout << "Distance = " << evo_mini->getDistance() << std::endl;
+  }
+
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  std::unique_ptr<terabee::ITerarangerFactory> factory;
+  std::unique_ptr<terabee::ITerarangerEvoMini> evo_mini;
+};
 
 int main(int argc, char **argv)
 {
-  (void) argc;
-  (void) argv;
 
-  printf("hello world height package\n");
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<HeightReader>());
+  rclcpp::shutdown();
+
   return 0;
 }

src/object_detection/CMakeLists.txt

@@ -23,7 +23,7 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(TerabeeApi REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
-find_package(object_detection REQUIRED)
+#find_package(object_detection REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/LidarReading.msg"
@@ -31,13 +31,13 @@ rosidl_generate_interfaces(${PROJECT_NAME}
  )
 
 add_executable(lidar_reader src/lidar_reader.cpp)
-ament_target_dependencies(lidar_reader rclcpp object_detection)
+ament_target_dependencies(lidar_reader rclcpp)
 
 target_link_libraries(lidar_reader ${TerabeeApi_LIBRARIES})
 target_include_directories(lidar_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS})
 
 add_executable(multiflex_reader src/multiflex_reader.cpp)
-ament_target_dependencies(multiflex_reader rclcpp object_detection)
+ament_target_dependencies(multiflex_reader rclcpp)
 
 target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES})
 target_include_directories(multiflex_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS})