UI + Bug fixes (#16)

* working in theory

* ui tool + some bug fixes

* Update README.md

---------

Co-authored-by: Thakker Aarsh <[email protected]>

.gitignore

@@ -2,4 +2,6 @@
 build/
 deps/NatNetSDK/*
 !deps/NatNetSDK/.gitkeep
-
+whiteboard/*
+config/*
+!config/initiate.yaml

CITATION.cff

@@ -1,9 +1,8 @@
 cff-version: 1.2.0
-message: "If you use this software, please cite it as below."
+message: 'If you use this software, please cite it as below.'
 
 title: Seamless integration of Optitrack motion capture with ROS
 type: software
-doi: 10.5281/zenodo.7773545
 conference: "Conférence annuelle des développeurs et utilisateurs ROS (ROSCon Fr 2022), Toulouse, June 23, 2022"
 license: GNU General Public License v3.0
 url: https://github.com/L2S-lab/natnet_ros_cpp
@@ -14,4 +13,5 @@ authors:
   - given-names: Aarsh
     family-names: THAKKER
     email: [email protected]
-    orcid: https://orcid.org/0000-0001-9638-5045
+
+

CMakeLists.txt

@@ -5,11 +5,13 @@ project(natnet_ros_cpp)
 find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   roscpp
+  rospy
   std_msgs
   visualization_msgs
   sensor_msgs
   tf2 
   tf2_ros
+  message_generation
 )
 find_package(Boost 1.65 REQUIRED COMPONENTS system thread)
 find_package(Eigen3 REQUIRED)
@@ -37,13 +39,31 @@ set(my_include_directories
   ${EIGEN3_INCLUDE_DIRS}
   ${NATNET_SDK_DIR}/include
 )
+set(QT_DEPEND
+    Qt5Widgets
+    Qt5PrintSupport
+    )
+set (QT_DEPEND_LIB
+    Qt5::Widgets
+    Qt5::Core
+    Qt5::PrintSupport
+    )
 ## Generate messages in the 'msg' folder
 # add_message_files(
 #   FILES
 #   Message1.msg
 #   Message2.msg
 # )
 
+add_service_files(
+  FILES
+  MarkerPoses.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
 ## Generate added messages and services with any dependencies listed here
 # generate_messages(
 #   DEPENDENCIES
@@ -53,7 +73,7 @@ set(my_include_directories
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES natnet_ros_cpp
-#  CATKIN_DEPENDS geometry_msgs roscpp std_msgs
+  CATKIN_DEPENDS geometry_msgs roscpp std_msgs message_runtime rospy std_msgs sensor_msgs
 #  DEPENDS system_lib
 )
 
@@ -78,6 +98,10 @@ include_directories(
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(${PROJECT_NAME} src/natnet_ros.cpp src/internal.cpp src/nn_filter.cpp)
 
+add_executable(marker_poses_server src/marker_poses_server.cpp)
+target_link_libraries(marker_poses_server ${catkin_LIBRARIES} ${NATNET_LIBRARY_DIR})
+add_dependencies(marker_poses_server ${PROJECT_NAME})
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
@@ -148,3 +172,7 @@ target_link_libraries(${PROJECT_NAME}
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+catkin_install_python(PROGRAMS scripts/helper_node.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

README.md

@@ -1,9 +1,6 @@
 # NatNet 4 ROS driver
 
-[![GitHub Action Status](https://github.com/L2S-lab/natnet_ros_cpp/actions/workflows/main.yaml/badge.svg?event=push)](https://github.com/L2S-lab/natnet_ros_cpp)  
-
-[![DOI](https://zenodo.org/badge/DOI/10.5281/zenodo.7773546.svg)](https://doi.org/10.5281/zenodo.7773546)
-
+[![GitHub Action Status](https://github.com/L2S-lab/natnet_ros_cpp/actions/workflows/main.yaml/badge.svg?event=push)](https://github.com/L2S-lab/natnet_ros_cpp)
 
 
 ## Introduction
@@ -28,7 +25,6 @@ This package is only tested with the Natnet 4.0 and ROS noetic but probably will
  - Considering compensation of time delay (~5-6ms) to the ROS time stamp.
  - Adding an option for the axis orientation (Z UP or Y UP)
  - Creating package for ROS2.
- - Implement CI-CD and Issue templete.
 
 ## How to use it
 
@@ -42,9 +38,8 @@ Keep your system connected to the internet while building the package for the fi
 cd ~/catkin_ws/src
 git clone https://github.com/L2S-lab/natnet_ros_cpp
 cd ..
-catkin_build  #OR catkin_make
+catkin build  #OR catkin_make
 . devel/setup.bash
-roslaunch natnet_ros_cpp natnet_ros.launch
 ```
 
 #### Setup the Motive for this package
@@ -60,22 +55,38 @@ Here is an example of how your streaming settings should look.
 
 ![alt text](https://github.com/L2S-lab/natnet_ros_cpp/blob/noeitc/img/streaming.png)
 
-#### Understanding the launch file
+
+#### Easy way
+
+Using GUI tool
+Here, you can use simple tool and follow the instruction from the output area on the right bottom corner.
+```
+roslaunch natnet_ros_cpp gui_natnet_ros.launch
+```
+![alt text](https://github.com/L2S-lab/natnet_ros_cpp/blob/noeitc/img/ui-1.png)
+
+
+#### Difficult way
+
+Using Non gui approach
+`roslaunch natnet_ros_cpp natnet_ros.launch`
+
+- ##### Understanding the launch file
 Launch file `natnet_ros.launch` contains the several configurable arguments. The details are mentioned in the launch file. Following are several important argument for the connection and the data transfer. Other connection arguments are for the advanced option.
 
 - `serverIP` : The IP address of the host PC. (The one selected in the Local Interface in Motive app)
 - `clientIP` : The IP address of the PC on which the file will be launched
 - `serverType` : Two possible options, `multicast` and `unicast`
 
-#### Publishing the single marker 
+- ##### Publishing the single marker 
 It is possible to track the single marker as a rigid body with constant orientation. Go to the `config/initiate.yaml` It is suggested to make a copy of the file and rename the new file.
 The file contains the details on what to modify. 
 
 The question might arise on how to check the position of the single marker. For that, you can log the frames of the incoming data in the terminal. To do so, enable the `log_frames` in the launch file.
 
 After configuring the `initiate.yaml`, in the launch file, enable the `pub_individual_marker`. Change the name of the config file in the argument `conf_file` if needed and launch the file.
 
-#### Replacing existing package
+- ##### Replacing existing package
 You can easily replace the current package with this package. In the `natnet_ros.launch` change the name of node to the node you currently using. For an example, 
 If you are using the `vrpn_client_node`
 changes are following

config/initiate.yaml

@@ -6,7 +6,7 @@ nearest_nbr: true
 kalman: false
 
 #-------------#
-#Parameters for nearest neighbour filter
+#Parameters for nearest neighbour filter, Best to use norm instead of individual error until you know what you what you are doing.
 individual_error: false
 # Tracking tolerance considered individually in x,y,z when 'Individual_error' is set to true
 # Ideally tolerance shoould be greater than the (max robot speed in m/s)/(frame rate of motion capture system in Hz) 
@@ -20,32 +20,56 @@ E: 0.04
 #-------------#
 
 # Add all the object names in this list to get the head count and the names of the objects
-object_names:
-  - crazyflie1
-  - crazyflie2
+#object_names:
+#  - crazyflie_1
+#  - crazyflie_2
+#   - rmtt_1
 
 # Initial position of the object must be within the tracking tolerance (in meters).
 # marker_config and orientation is not used as of now. 
-crazyflie1:
-  marker_config: 0 
-  pose: 
-    position:
-      - 0.0
-      - 0.0
-      - 0.048
-    orientation:
-      - 0.0
-      - 0.0
-      - 0.0
+#crazyflie_1:
+#  marker_config: 0 
+#  pose: 
+#    position:
+#      - 0.0
+#      - 0.0
+#      - 0.048
+#    orientation:
+#      - 0.0
+#      - 0.0
+#      - 0.0
+
+#crazyflie_2:
+#  marker_config: 0
+#  pose: 
+#    position:
+#      - 0.0
+#      - -1.75
+#      - 0.042
+#    orientation:
+#      - 0.0
+#      - 0.0
+#      - 0.0
 
-crazyflie2:
-  marker_config: 0
-  pose: 
-    position:
-      - 0.0
-      - -1.75
-      - 0.042
-    orientation:
-      - 0.0
-      - 0.0
-      - 0.0
+#rmtt_1:
+#  marker_config: 0
+#  pose:  
+#    position:
+#      - 0.0
+#      - 0.0
+#      - 0.094
+#    orientation:
+#      - 0.0
+#      - 0.0
+#      - 0.0
+
+object_names:
+  - cf1
+cf1:
+    marker_config: 0
+    pose:
+      orientation: []
+      position:
+      - -0.24554595351219177
+      - -1.3885701894760132
+      - 0.048130884766578674

include/natnet_ros_cpp/internal.h

@@ -31,9 +31,9 @@ class Internal
     std::map<std::string, ros::Publisher> RigidbodyPub;
     std::map<std::string, ros::Publisher> RigidbodyMarkerPub;
     std::map<std::string, ros::Publisher> IndividualMarkerPub;
-    std::map<std::string, ros::Publisher> SkeletonPub;
-    std::map<std::string, ros::Publisher> ForcePlatePub;
-    std::map<std::string, ros::Publisher> DevicePub;
+    std::map<std::string, ros::Publisher> SkeletonPub; // WIP : Need Help
+    std::map<std::string, ros::Publisher> ForcePlatePub; // WIP : Need Help
+    std::map<std::string, ros::Publisher> DevicePub; // WIP : Need Help
     ros::Publisher PointcloudPub;
     sensor_msgs::PointCloud msgPointcloud; // point cloud msg