5 #ifndef ROS_QUALISYS_QUALISYS_TO_ROS_HPP 6 #define ROS_QUALISYS_QUALISYS_TO_ROS_HPP 8 #include <geometry_msgs/TransformStamped.h> 10 #include <tf2/LinearMath/Matrix3x3.h> 11 #include <tf2/LinearMath/Quaternion.h> 12 #include <tf2_ros/transform_broadcaster.h> 15 #include "RTProtocol.h" 37 ros::NodeHandle* node_handle_;
40 CRTProtocol crt_protocol_;
43 std::string server_address_;
44 unsigned short base_port_;
45 unsigned short udp_port_;
52 std::string fixed_frame_id_;
55 bool data_available_ =
false;
58 CRTPacket::EPacketType packet_type_;
59 CRTPacket* rt_packet_;
60 unsigned int body_count_;
61 const char* body_name_;
65 std::array<float, 9> rotation_matrix_;
68 std::shared_ptr<ros::Rate> rate_;
69 tf2::Matrix3x3 ros_rotation_matrix_;
70 tf2::Quaternion ros_quaternion_;
71 geometry_msgs::TransformStamped ros_transform_;
72 tf2_ros::TransformBroadcaster publisher_;
77 #endif // ROS_QUALISYS_QUALISYS_TO_ROS_HPP void setNodeHandle(ros::NodeHandle &nh)
Definition: qualisys-to-ros.cpp:15
void run()
Definition: qualisys-to-ros.cpp:95
bool initialize()
Definition: qualisys-to-ros.cpp:19
void terminate()
Definition: qualisys-to-ros.cpp:170
QualisysToRos()
Definition: qualisys-to-ros.cpp:11
Definition: qualisys-to-ros.hpp:19
Definition: qualisys-to-ros.hpp:17
~QualisysToRos()
Definition: qualisys-to-ros.cpp:13