cmake_minimum_required(VERSION 3.16)
project(cpp_robot_tf2 VERSION 0.1.0)

if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

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(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

# --- 节点 1:joint_state_publisher ---
# 发布 sensor_msgs/JointState,周期 ~100ms,内容为每个关节的当前角度。
# robot_state_publisher 节点(系统包)会消费 /joint_states + robot_description,
# 自动算出 base_link -> link1 -> link2 -> gripper 的 TF 链。
add_executable(joint_state_publisher src/joint_state_publisher.cpp)
ament_target_dependencies(joint_state_publisher
  rclcpp sensor_msgs geometry_msgs
  tf2 tf2_ros tf2_geometry_msgs
)

# --- 节点 2:tf2_listener ---
# 用 tf2::Buffer + TransformListener 监听 base_link -> gripper 的变换,
# 周期性打印抓取点在 base_link 系下的 (x, y, z)。
add_executable(tf2_listener src/tf2_listener.cpp)
ament_target_dependencies(tf2_listener
  rclcpp sensor_msgs geometry_msgs
  tf2 tf2_ros tf2_geometry_msgs
)

install(TARGETS
  joint_state_publisher
  tf2_listener
  DESTINATION lib/${PROJECT_NAME}
)

# 把 urdf/ 整个目录安装到 share/<pkg>/urdf/,让 launch 可以读到 URDF。
install(DIRECTORY urdf
  DESTINATION share/${PROJECT_NAME}/
)

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}/
)

# --- 测试 ---
if(BUILD_TESTING)
  find_package(ament_cmake_gtest REQUIRED)
  ament_add_gtest(test_tf2_lookup test/test_tf2_lookup.cpp)
  ament_target_dependencies(test_tf2_lookup
    rclcpp sensor_msgs geometry_msgs
    tf2 tf2_ros tf2_geometry_msgs
  )
endif()

ament_package()