fix the implementation

CMakeLists.txt

@@ -87,3 +87,5 @@ install(
   DESTINATION lib)
 install(TARGETS ${PROJECT_NAME}-node RUNTIME DESTINATION lib/${PROJECT_NAME})
 install(FILES package.xml DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

config/bauzil-qualisys.yaml

+ros_qualisys:
+  server_address: "140.93.16.160"
+  server_base_port: 22222
+  server_udp_port: 6734
+  major_version: 1
+  minor_version: 20
+  frame_rate: 1000
+  fixed_frame_id: "mocap"

include/ros-qualisys/qualisys-to-ros.hpp

@@ -46,7 +46,6 @@ class QualisysToRos {
   int minor_version_;
   int major_version_;
   bool big_endian_;
-  std::vector<std::string> model_list_;
   int frame_rate_;
   double max_accel_;
   bool publish_tf_;

launch/qualisys_bauzil_bringup.launch

+<launch>
+  <rosparam command="load" file="$(find ros-qualisys)/config/bauzil-qualisys.yaml" />
+
+  <node name="ros_qualisys" pkg="ros-qualisys" type="ros-qualisys" output="screen">
+  </node>
+
+</launch>

src/qualisys-to-ros.cpp

@@ -29,7 +29,6 @@ bool QualisysToRos::initialize() {
   node_handle_->param("server_udp_port", tmp_udp_port, 6734);
   node_handle_->param("major_version", major_version_, 1);
   node_handle_->param("minor_version", minor_version_, 20);
-  node_handle_->param("model_list", model_list_, std::vector<std::string>(0));
   node_handle_->param("frame_rate", frame_rate_, 100);
   node_handle_->param("fixed_frame_id", fixed_frame_id_, std::string("mocap"));
   base_port_ = static_cast<unsigned short>(tmp_base_port);
@@ -53,7 +52,9 @@ bool QualisysToRos::connect() {
 
   // connect properly
   int failure = 0;
-  while (failure < 20 && ros::ok()) {
+  bool success = false;
+  while (failure < 20 && ros::ok() && !success) {
+    ROS_INFO_STREAM("QualisysToRos::connect(): Try to connect");
     if (!crt_protocol_.Connected()) {
       if (!crt_protocol_.Connect(server_address_.c_str(), base_port_,
                                  &udp_port_, major_version_, minor_version_,
@@ -66,7 +67,7 @@ bool QualisysToRos::connect() {
         continue;
       }
     }
-
+    ROS_INFO_STREAM("QualisysToRos::connect(): Are data available?");
     if (!data_available_) {
       if (!crt_protocol_.Read6DOFSettings(data_available_)) {
         ROS_INFO_STREAM("crt_protocol_.Read6DOFSettings: "
@@ -76,7 +77,7 @@ bool QualisysToRos::connect() {
         continue;
       }
     }
-
+    ROS_INFO_STREAM("QualisysToRos::connect(): Are data streamed");
     if (!crt_protocol_.StreamFrames(CRTProtocol::RateAllFrames, 0, 0, NULL,
                                     CRTProtocol::cComponent6d)) {
       ROS_INFO_STREAM(
@@ -85,8 +86,10 @@ bool QualisysToRos::connect() {
       sleep(1);
       continue;
     }
+    success = true;
   }
-  return failure >= 20;
+  ROS_INFO_STREAM("QualisysToRos::connect(): Connected successfully");
+  return failure >= 20 && success;
 }
 
 void QualisysToRos::run() {
@@ -95,8 +98,7 @@ void QualisysToRos::run() {
 
   rt_packet_ = crt_protocol_.GetRTPacket();
   body_count_ = rt_packet_->Get6DOFBodyCount();
-
-  ROS_INFO_STREAM("Number of bodies found :" << body_count_);
+  ROS_INFO_THROTTLE(1, "Number of bodies found :%d", body_count_);
 
   if (crt_protocol_.Receive(packet_type_, true) ==
       CNetwork::ResponseType::success) {
@@ -136,7 +138,7 @@ void QualisysToRos::run() {
               ros_transform_.transform.rotation.y = ros_quaternion_.y();
               ros_transform_.transform.rotation.z = ros_quaternion_.z();
               ros_transform_.transform.rotation.w = ros_quaternion_.w();
-              ros_transform_.header.frame_id = "Qualisys";
+              ros_transform_.header.frame_id = fixed_frame_id_;
               ros_transform_.header.stamp = ros::Time::now();
               ros_transform_.child_frame_id = body_name_;
               publisher_.sendTransform(ros_transform_);

src/ros-qualisys.cpp

@@ -7,8 +7,8 @@
 #include "ros-qualisys/qualisys-to-ros.hpp"
 
 int main(int argc, char* argv[]) {
-  ros::init(argc, argv, "ros_qualisys", ros::init_options::NoSigintHandler);
-  ros::NodeHandle nh;
+  ros::init(argc, argv, "ros-qualisys", ros::init_options::NoSigintHandler);
+  ros::NodeHandle nh("~");
 
   ros_qualisys::QualisysToRos q2r;
   q2r.setNodeHandle(nh);