qualisys-to-ros.hpp
Go to the documentation of this file.
1
5#ifndef ROS_QUALISYS_QUALISYS_TO_ROS_HPP
6#define ROS_QUALISYS_QUALISYS_TO_ROS_HPP
7
8#include <geometry_msgs/TransformStamped.h>
9#include <ros/ros.h>
10#include <tf2/LinearMath/Matrix3x3.h>
11#include <tf2/LinearMath/Quaternion.h>
12#include <tf2_ros/transform_broadcaster.h>
13
14#include "RTPacket.h"
15#include "RTProtocol.h"
16
17namespace ros_qualisys {
18
20 public:
22
24
25 void setNodeHandle(ros::NodeHandle& nh);
26
27 bool initialize();
28
29 void run();
30
31 void terminate();
32
33 private:
34 bool connect();
35
36 private:
37 ros::NodeHandle* node_handle_;
38
39 // Qualisys objects:
40 CRTProtocol crt_protocol_;
41
42 // params
43 std::string server_address_;
44 unsigned short base_port_;
45 unsigned short udp_port_;
46 int minor_version_;
47 int major_version_;
48 bool big_endian_;
49 int frame_rate_;
50 double max_accel_;
51 bool publish_tf_;
52 std::string fixed_frame_id_;
53
54 // sanity check variables
55 bool data_available_ = false;
56
57 // acquiring data
58 CRTPacket::EPacketType packet_type_;
59 CRTPacket* rt_packet_;
60 unsigned int body_count_;
61 const char* body_name_;
62 float px_;
63 float py_;
64 float pz_;
65 std::array<float, 9> rotation_matrix_;
66
67 // ROS interface
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_;
73};
74
75} // namespace ros_qualisys
76
77#endif // ROS_QUALISYS_QUALISYS_TO_ROS_HPP
Definition: qualisys-to-ros.hpp:19
void run()
Definition: qualisys-to-ros.cpp:95
void setNodeHandle(ros::NodeHandle &nh)
Definition: qualisys-to-ros.cpp:15
QualisysToRos()
Definition: qualisys-to-ros.cpp:11
bool initialize()
Definition: qualisys-to-ros.cpp:19
void terminate()
Definition: qualisys-to-ros.cpp:170
~QualisysToRos()
Definition: qualisys-to-ros.cpp:13
Definition: qualisys-to-ros.hpp:17