why
This commit is contained in:
@@ -24,14 +24,16 @@ find_package(rclcpp REQUIRED)
|
|||||||
find_package(std_msgs REQUIRED)
|
find_package(std_msgs REQUIRED)
|
||||||
find_package(positioning_systems_api REQUIRED)
|
find_package(positioning_systems_api REQUIRED)
|
||||||
find_package(rosidl_default_generators REQUIRED)
|
find_package(rosidl_default_generators REQUIRED)
|
||||||
#find_package(beacon_positioning REQUIRED)
|
find_package(beacon_positioning REQUIRED)
|
||||||
|
|
||||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||||
"msg/TrackerPosition.msg" # message for tracker position
|
"msg/TrackerPosition.msg" # message for tracker position
|
||||||
)
|
)
|
||||||
|
|
||||||
add_executable(tracker_position src/tracker_position.cpp)
|
add_executable(tracker_position src/tracker_position.cpp)
|
||||||
ament_target_dependencies(tracker_position rclcpp std_msgs)
|
ament_target_dependencies(tracker_position rclcpp std_msgs
|
||||||
|
beacon_positioning
|
||||||
|
)
|
||||||
|
|
||||||
target_link_libraries(tracker_position
|
target_link_libraries(tracker_position
|
||||||
positioning_systems_api::serial_communication
|
positioning_systems_api::serial_communication
|
||||||
|
|||||||
@@ -63,18 +63,20 @@ private:
|
|||||||
{
|
{
|
||||||
auto msg = height::msg::HeightData();
|
auto msg = height::msg::HeightData();
|
||||||
|
|
||||||
float min = 10000000;
|
float min = 255;
|
||||||
terabee::DistanceData distance_data = evo_mini->getDistance();
|
terabee::DistanceData distance_data = evo_mini->getDistance();
|
||||||
for (size_t i = 0; i < distance_data.size(); i++)
|
for (size_t i = 0; i < distance_data.size(); i++)
|
||||||
{
|
{
|
||||||
msg.heights[i] = distance_data.distance[i];
|
msg.heights[i] = distance_data.distance[i];
|
||||||
if (distance_data.distance[i] < min)
|
if (distance_data.distance[i] < min && distance_data.distance[i] >= 0)
|
||||||
{
|
{
|
||||||
min = distance_data.distance[i];
|
min = distance_data.distance[i];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
msg.min_height = min;
|
msg.min_height = min;
|
||||||
publisher_->publish(msg);
|
publisher_->publish(msg);
|
||||||
|
|
||||||
|
RCLCPP_INFO(this->get_logger(),"publishing message with min distance %f",msg.min_height);
|
||||||
}
|
}
|
||||||
|
|
||||||
rclcpp::TimerBase::SharedPtr timer_;
|
rclcpp::TimerBase::SharedPtr timer_;
|
||||||
|
|||||||
@@ -31,13 +31,17 @@ 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
|
||||||
|
)
|
||||||
|
|
||||||
target_link_libraries(lidar_reader ${TerabeeApi_LIBRARIES})
|
target_link_libraries(lidar_reader ${TerabeeApi_LIBRARIES})
|
||||||
target_include_directories(lidar_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS})
|
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
|
||||||
|
)
|
||||||
|
|
||||||
target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES})
|
target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES})
|
||||||
target_include_directories(multiflex_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS})
|
target_include_directories(multiflex_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS})
|
||||||
|
|||||||
Reference in New Issue
Block a user