#include "arm_driver_define.hpp"
#include "rm_arm_node.hpp"

int main(int argc, char *argv[]) {
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Starting rm_arm_control node...");

    rclcpp::init(argc, argv);

    ArmDriver *armDriver = NULL;
    Kinematics *armKinematics = NULL;
    int initResult = ArmInitFunc(&armDriver, &armKinematics);
    if (initResult != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize rm arm driver. %d", initResult);
        return UNKNOWN_ERR;
    }

    auto rm_arm_control = std::make_shared<RmArmNode>(armDriver, armKinematics);

    rclcpp::spin(rm_arm_control);

    rclcpp::shutdown();
    return 0;
}