sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

everything built

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-25 09:11:44 +00:00
parent 48b5016897
commit 9af542d4ad
3 changed files with 19 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/beacon_positioning/src/tracker_position.cpp
View File

@@ -149,7 +149,7 @@ int main(int argc, char **argv)
if (tracker_msg.is_valid_position) if (tracker_msg.is_valid_position)
{ {
RCLCPP_INFO(node->get_logger(), "x = %f, y = %f, z = %f", tracker_msg.tracker_position_xyz.at(0), tracker_msg.tracker_position_xyz.at(1), tracker_msg.tracker_position_xyz.at(2)); RCLCPP_INFO(node->get_logger(), "x = %f, y = %f, z = %f", tracker_msg.tracker_position_xyz.at(0), tracker_msg.tracker_position_xyz.at(1), tracker_msg.tracker_position_xyz.at(2));
for (int i = 0; i < tracker_msg.anchors_data.size(); i++) for (size_t i = 0; i < tracker_msg.anchors_data.size(); i++)
{ {
RCLCPP_INFO(node->get_logger(), "anchor number= %d, distance = %d, x = %d, y = %d, z = %d", tracker_msg.anchors_data[i].number, tracker_msg.anchors_data[i].distance, tracker_msg.anchors_data[i].pos_x, tracker_msg.anchors_data[i].pos_y, tracker_msg.anchors_data[i].pos_z); RCLCPP_INFO(node->get_logger(), "anchor number= %d, distance = %d, x = %d, y = %d, z = %d", tracker_msg.anchors_data[i].number, tracker_msg.anchors_data[i].distance, tracker_msg.anchors_data[i].pos_x, tracker_msg.anchors_data[i].pos_y, tracker_msg.anchors_data[i].pos_z);
} }
@@ -160,7 +160,7 @@ int main(int argc, char **argv)
message.y_pos = tracker_msg.tracker_position_xyz.at(1); message.y_pos = tracker_msg.tracker_position_xyz.at(1);
message.z_pos = tracker_msg.tracker_position_xyz.at(2); message.z_pos = tracker_msg.tracker_position_xyz.at(2);
message.anchor_distances = {0, 0, 0, 0}; message.anchor_distances = {0, 0, 0, 0};
for (int i = 0; i < tracker_msg.anchors_data.size(); i++) for (size_t i = 0; i < tracker_msg.anchors_data.size(); i++)
{ {
message.anchor_distances[i] = tracker_msg.anchors_data[i].distance; message.anchor_distances[i] = tracker_msg.anchors_data[i].distance;
} }

6
src/object_detection/CMakeLists.txt
View File

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

28
src/object_detection/src/lidar_reader.cpp
View File

@@ -57,7 +57,7 @@ public:
this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor); this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor);
publisher_ = this->create_publisher<object_detection::msg::LidarReading>("drone/object_detection", 10); // publisher_ = this->create_publisher<object_detection::msg::LidarReading>("drone/object_detection", 10);
timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this)); timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this));
ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc); ITerarangerTowerEvo::ImuMode mode(ITerarangerTowerEvo::QuaternionLinearAcc);
@@ -87,24 +87,24 @@ private:
// std::cout << "Distance = " << tower->getDistance() << std::endl; // std::cout << "Distance = " << tower->getDistance() << std::endl;
// std::cout << "IMU = " << tower->getImuData() << std::endl; // std::cout << "IMU = " << tower->getImuData() << std::endl;
auto msg = object_detection::msg::LidarReading(); // auto msg = object_detection::msg::LidarReading();
msg.sensor_1 = tower->getDistance().distance.at(0); // msg.sensor_1 = tower->getDistance().distance.at(0);
msg.sensor_2 = tower->getDistance().distance.at(2); // msg.sensor_2 = tower->getDistance().distance.at(2);
msg.sensor_3 = tower->getDistance().distance.at(4); // msg.sensor_3 = tower->getDistance().distance.at(4);
msg.sensor_4 = tower->getDistance().distance.at(6); // msg.sensor_4 = tower->getDistance().distance.at(6);
ImuData imu_data = tower->getImuData(); // ImuData imu_data = tower->getImuData();
for (size_t i = 0; i < imu_data.data.size(); i++) // for (size_t i = 0; i < imu_data.data.size(); i++)
{ // {
msg.imu_data.push_back(imu_data.data[i]); // msg.imu_data.push_back(imu_data.data[i]);
} // }
publisher_->publish(msg); // // publisher_->publish(msg);
RCLCPP_INFO(this->get_logger(), "Publishing message"); // RCLCPP_INFO(this->get_logger(), "Publishing message");
} }
rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_; // rclcpp::Publisher<object_detection::msg::LidarReading>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
// terabee tower evo variables // terabee tower evo variables