00001 #include <ros/ros.h> 00002 #include <ros/spinner.h> 00003 #include "epos_hardware/epos_hardware.h" 00004 #include <controller_manager/controller_manager.h> 00005 #include <vector> 00006 00007 int main(int argc, char** argv) { 00008 ros::init(argc, argv, "epos_velocity_hardware"); 00009 ros::NodeHandle nh; 00010 ros::NodeHandle pnh("~"); 00011 00012 std::vector<std::string> motor_names; 00013 for(int i = 0; i < argc-1; ++i) { 00014 motor_names.push_back(argv[i+1]); 00015 } 00016 epos_hardware::EposHardware robot(nh, pnh, motor_names); 00017 controller_manager::ControllerManager cm(&robot, nh); 00018 00019 ros::AsyncSpinner spinner(1); 00020 spinner.start(); 00021 00022 ROS_INFO("Initializing Motors"); 00023 if(!robot.init()) { 00024 ROS_FATAL("Failed to initialize motors"); 00025 return 1; 00026 } 00027 ROS_INFO("Motors Initialized"); 00028 00029 ros::Rate controller_rate(50); 00030 ros::Time last = ros::Time::now(); 00031 while (ros::ok()) { 00032 robot.read(); 00033 ros::Time now = ros::Time::now(); 00034 cm.update(now, now-last); 00035 robot.write(); 00036 last = now; 00037 robot.update_diagnostics(); 00038 controller_rate.sleep(); 00039 } 00040 00041 }