102 lines
2.7 KiB
C++
102 lines
2.7 KiB
C++
#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;
|
|
}
|