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;
}
