move height and lidar message definitions to drone_services package
This commit is contained in:
@@ -34,6 +34,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/DroneArmStatus.msg"
|
||||
"msg/DroneRouteStatus.msg"
|
||||
"msg/DroneStatus.msg"
|
||||
"msg/HeightData.msg"
|
||||
"msg/LidarReading.msg"
|
||||
"msg/MultiflexReading.msg"
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
|
||||
@@ -19,12 +19,7 @@ endif()
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(TerabeeApi REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(height REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/HeightData.msg"
|
||||
)
|
||||
find_package(drone_services REQUIRED)
|
||||
|
||||
add_executable(height_reader src/height_reader.cpp)
|
||||
target_include_directories(height_reader PUBLIC
|
||||
@@ -38,7 +33,7 @@ ament_target_dependencies(
|
||||
height_reader
|
||||
rclcpp
|
||||
TerabeeApi
|
||||
height
|
||||
drone_services
|
||||
)
|
||||
|
||||
install(TARGETS height_reader
|
||||
|
||||
@@ -7,7 +7,7 @@
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "height/msg/height_data.hpp"
|
||||
#include "drone_services/msg/height_data.hpp"
|
||||
|
||||
#include <terabee/ITerarangerFactory.hpp>
|
||||
#include <terabee/ITerarangerEvoMini.hpp>
|
||||
@@ -48,7 +48,7 @@ public:
|
||||
RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!");
|
||||
|
||||
timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this));
|
||||
publisher_ = this->create_publisher<height::msg::HeightData>("drone/height", 10);
|
||||
publisher_ = this->create_publisher<drone_services::msg::HeightData>("drone/height", 10);
|
||||
}
|
||||
|
||||
private:
|
||||
@@ -58,7 +58,7 @@ private:
|
||||
*/
|
||||
void read_height()
|
||||
{
|
||||
auto msg = height::msg::HeightData();
|
||||
auto msg = drone_services::msg::HeightData();
|
||||
|
||||
// high initial minimal value
|
||||
float min = 255;
|
||||
@@ -82,7 +82,7 @@ private:
|
||||
// timer for publishing height readings
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
// publisher for height readings
|
||||
rclcpp::Publisher<height::msg::HeightData>::SharedPtr publisher_;
|
||||
rclcpp::Publisher<drone_services::msg::HeightData>::SharedPtr publisher_;
|
||||
|
||||
// factory for height sensor
|
||||
std::unique_ptr<terabee::ITerarangerFactory> factory;
|
||||
|
||||
@@ -22,17 +22,11 @@ find_package(ament_cmake REQUIRED)
|
||||
# find_package(<dependency> REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(TerabeeApi REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(object_detection REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/LidarReading.msg"
|
||||
"msg/MultiflexReading.msg"
|
||||
)
|
||||
find_package(drone_services REQUIRED)
|
||||
|
||||
add_executable(lidar_reader src/lidar_reader.cpp)
|
||||
ament_target_dependencies(lidar_reader rclcpp
|
||||
object_detection
|
||||
drone_services
|
||||
)
|
||||
|
||||
target_link_libraries(lidar_reader ${TerabeeApi_LIBRARIES})
|
||||
@@ -40,7 +34,7 @@ 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
|
||||
drone_services
|
||||
)
|
||||
|
||||
target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES})
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "object_detection/msg/lidar_reading.hpp"
|
||||
#include "drone_services/msg/lidar_reading.hpp"
|
||||
|
||||
#include <terabee/ITerarangerFactory.hpp>
|
||||
#include <terabee/ITerarangerTowerEvo.hpp>
|
||||
@@ -26,7 +26,7 @@ public:
|
||||
this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor);
|
||||
|
||||
// create publisher and bind timer to publish function
|
||||
publisher_ = this->create_publisher<object_detection::msg::LidarReading>("drone/object_detection", 10);
|
||||
publisher_ = this->create_publisher<drone_services::msg::LidarReading>("drone/object_detection", 10);
|
||||
timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this));
|
||||
|
||||
factory = terabee::ITerarangerFactory::getFactory();
|
||||
@@ -50,7 +50,7 @@ public:
|
||||
|
||||
private:
|
||||
// publisher for lidar data
|
||||
rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_;
|
||||
rclcpp::Publisher<drone_services::msg::LidarReading>::SharedPtr publisher_;
|
||||
// timer for publishing readings
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
|
||||
@@ -64,7 +64,7 @@ private:
|
||||
*/
|
||||
void read_lidar_data()
|
||||
{
|
||||
auto msg = object_detection::msg::LidarReading();
|
||||
auto msg = drone_services::msg::LidarReading();
|
||||
|
||||
// read distance data from all sensors
|
||||
msg.sensor_1 = tower->getDistance().distance.at(0);
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "object_detection/msg/multiflex_reading.hpp"
|
||||
#include "drone_services/msg/multiflex_reading.hpp"
|
||||
|
||||
#include <terabee/ITerarangerFactory.hpp>
|
||||
#include <terabee/ITerarangerMultiflex.hpp>
|
||||
@@ -48,7 +48,7 @@ public:
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Multiflex initialized");
|
||||
|
||||
publisher_ = this->create_publisher<object_detection::msg::MultiflexReading>("drone/object_detection", 10);
|
||||
publisher_ = this->create_publisher<drone_services::msg::MultiflexReading>("drone/object_detection", 10);
|
||||
timer_ = this->create_wall_timer(500ms, std::bind(&MultiflexReader::read_multiflex_data, this));
|
||||
}
|
||||
|
||||
@@ -59,7 +59,7 @@ private:
|
||||
|
||||
// timer and publisher for publishing message onto topic
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
rclcpp::Publisher<object_detection::msg::MultiflexReading>::SharedPtr publisher_;
|
||||
rclcpp::Publisher<drone_services::msg::MultiflexReading>::SharedPtr publisher_;
|
||||
|
||||
/**
|
||||
* @brief Reads distance data from the sensor and publishes it onto the drone/object_detection topic
|
||||
@@ -71,7 +71,7 @@ private:
|
||||
terabee::DistanceData data = multiflex->getDistance();
|
||||
|
||||
// populate message with readings
|
||||
auto msg = object_detection::msg::MultiflexReading();
|
||||
auto msg = drone_services::msg::MultiflexReading();
|
||||
for (size_t i = 0; i < data.size(); i++)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "distance %f", data.distance[i]);
|
||||
|
||||
Reference in New Issue
Block a user