#include "rclcpp/rclcpp.hpp"
#include "interfaces/msg/imu_msg.hpp"
#include "sensor_msgs/msg/imu.hpp"
using namespace std;
int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("imu_dev_node");
  auto imu_pub = node->create_publisher<interfaces::msg::ImuMsg>("/imu_msg", 10);
  auto imu_sub=node->create_subscription<sensor_msgs::msg::Imu>("/imu/data", 10, [=](const sensor_msgs::msg::Imu::SharedPtr msg){
      //cout<<msg->linear_acceleration.x<<","<<msg->linear_acceleration.y<<","<<msg->linear_acceleration.z<<endl;
      //cout<<msg->angular_velocity.x<<","<<msg->angular_velocity.y<<","<<msg->angular_velocity.z<<endl;
      //cout<<msg->orientation.x<<","<<msg->orientation.y<<","<<msg->orientation.z<<","<<msg->orientation.w<<endl;
      interfaces::msg::ImuMsg imu_msg;
      imu_msg.source="waveshare";
      imu_msg.position="front";
      imu_msg.type="None";
      imu_msg.imu=*msg;
      imu_pub->publish(imu_msg);
  });
  rclcpp::spin(node);
  rclcpp::shutdown();
}
