int main(int argc, char** argv) { // Init the ROS node ros::init(argc, argv, "omnirob_robin_lwa_interface"); ros::NodeHandle n; ros::Rate loop_rate(100); // 100 Hz => consistent to sampleTime ros::AsyncSpinner spinner(4); // Use 4 threads //MyRobot omniRob; controller_manager::ControllerManager cm(&omniRob); ros::Duration sampleTime(0.01); ros::Publisher trajectoryPoint_pub = n.advertise<std_msgs::Float64MultiArray>("lwa/control/commanded_joint_state", 1000); ros::Subscriber jointState_sub = n.subscribe("lwa/state/joint_state", 1000, jointState_callback); std_msgs::Float64MultiArray trajPoint; spinner.start(); while (ros::ok()) { //omniRob.read(); // automated subscription cm.update(ros::Time::now(), sampleTime); omniRob.write(trajectoryPoint_pub, trajPoint); ROS_INFO("omnirob LWA3 OK..."); //ros::spinOnce(); //ros::waitForShutdown(); loop_rate.sleep(); //spinner.stop(); } return 0; }
int main(int argc, char** argv) { ros::init(argc, argv, APPLICATION_NAME); // Override the default ros sigint handler. // This must be set after the first NodeHandle is created. signal(SIGINT, mySigintHandler); ros::NodeHandle nh; // ros::CallbackQueue queue; // nh.setCallbackQueue(&queue); // Allow the action server to recieve and send ros messages ros::AsyncSpinner spinner(4); spinner.start(); // To create publisher and subscriber... do we really need this node here?? // ros::NodeHandle nodeHandle; std::cout << "Before MyRobot"<< std::endl; MyRobot robot; if(!robot.init() ) { std::cout << "Error initializing robot model, quitting" << std::endl; return -1; } controller_manager::ControllerManager cm(&robot, nh); ros::Duration elapsed_time_(1,0); while (ros::ok()) { std::cout << "Updating stuff" << std::endl; robot.read(); cm.update(ros::Time::now(), elapsed_time_); robot.write(); sleep(1); ros::spinOnce(); } return 0; }