git commit -m "first commit for v2"

Controllers/Packages/nova5_control/example/test_thread.cpp

@@ -0,0 +1,101 @@
+#include "nova5_control/imr_nova_control.h"
+#include <iostream>
+#include <memory>
+#include <ros/ros.h>
+#include <std_msgs/UInt16.h>
+
+std::shared_ptr<imr_nova_control> controller = nullptr;
+
+unsigned int ok_count_max = 0;
+unsigned int ng_count_max = 1;
+bool enable = true;
+bool continue_flag = false;
+bool go_home_flag = false;
+bool power_on_flag = false;
+
+void callBack(const std_msgs::UInt16::ConstPtr& msg)
+{
+    if(controller != nullptr)
+    {
+        switch (msg->data)
+        {
+        case 1:
+        controller->startHomeThread();
+            // continue_flag = true;
+            // go_home_flag = false;
+            // power_on_flag = false;
+            break;
+        case 2:
+            // continue_flag = false;
+            // go_home_flag = true;
+            // power_on_flag = false;
+            break;
+        case 3:
+            // continue_flag = false;
+            // go_home_flag = false;
+            // power_on_flag = true;
+            break;        
+        default:
+            // continue_flag = false;
+            // go_home_flag = false;
+            // power_on_flag = false;
+            break;
+        }
+    }
+}
+
+int main(int argc, char** argv) {
+    // Initialize the ROS node
+    ros::init(argc, argv, "test_imr_nova_control");
+    ros::NodeHandle nh = ros::NodeHandle("~");
+    ros::Subscriber sub = nh.subscribe("mode", 1, callBack);
+    ros::Publisher pub = nh.advertise<std_msgs::UInt16>("status", 1);
+
+    try {
+        // Initialize ROS time
+        // ros::Time::init();
+
+        // Create an instance of imr_nova_control
+        controller = std::make_shared<imr_nova_control>();
+
+        
+
+        controller->ok_count_max_ = &ok_count_max;
+        controller->ng_count_max_ = &ng_count_max;
+        controller->enable_ = &enable;
+
+        controller->continue_flag_ = &continue_flag;
+        controller->go_home_flag_ = &go_home_flag;
+        controller->power_on_flag_ = &power_on_flag;
+
+        
+        #if 0
+        controller->startHomeThread();
+        #endif
+
+        #if 0
+        controller->startModeThread();
+        #endif
+
+        std::cout << "imr_nova_control instance created successfully." << std::endl;
+
+        // Run ROS spin to process callbacks (if applicable)
+        // ros::AsyncSpinner spinner(2); // Adjust the number of threads as needed
+        // spinner.start();
+        ros::Rate rate(5);
+
+        while (ros::ok())
+        {
+            std_msgs::UInt16 msg;
+            msg.data = controller->statusCode_;
+            pub.publish(msg);
+            ros::spinOnce();
+            rate.sleep();
+        }
+
+    } catch (const std::exception& e) {
+        std::cerr << "An error occurred: " << e.what() << std::endl;
+    }
+    ros::spin();
+    return 0;
+}