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 
17 namespace ros_qualisys {
18 
20  public:
21  QualisysToRos();
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
ros_qualisys::QualisysToRos
Definition: qualisys-to-ros.hpp:19
ros_qualisys::QualisysToRos::initialize
bool initialize()
Definition: qualisys-to-ros.cpp:19
ros_qualisys
Definition: qualisys-to-ros.hpp:17
ros_qualisys::QualisysToRos::terminate
void terminate()
Definition: qualisys-to-ros.cpp:170
ros_qualisys::QualisysToRos::QualisysToRos
QualisysToRos()
Definition: qualisys-to-ros.cpp:11
ros_qualisys::QualisysToRos::setNodeHandle
void setNodeHandle(ros::NodeHandle &nh)
Definition: qualisys-to-ros.cpp:15
ros_qualisys::QualisysToRos::run
void run()
Definition: qualisys-to-ros.cpp:95
ros_qualisys::QualisysToRos::~QualisysToRos
~QualisysToRos()
Definition: qualisys-to-ros.cpp:13