Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <chrono>
00035
00036 #include <kuka_eki_hw_interface/kuka_eki_hw_interface.h>
00037
00038 int main(int argc, char** argv)
00039 {
00040 ros::init(argc, argv, "kuka_eki_hw_interface");
00041
00042 ros::AsyncSpinner spinner(2);
00043 spinner.start();
00044
00045 ros::NodeHandle nh;
00046
00047 kuka_eki_hw_interface::KukaEkiHardwareInterface hardware_interface;
00048 hardware_interface.init();
00049
00050
00051 ros::Time timestamp;
00052 ros::Duration period;
00053 auto stopwatch_last = std::chrono::steady_clock::now();
00054 auto stopwatch_now = stopwatch_last;
00055
00056 controller_manager::ControllerManager controller_manager(&hardware_interface, nh);
00057
00058 hardware_interface.start();
00059
00060
00061 timestamp = ros::Time::now();
00062 stopwatch_now = std::chrono::steady_clock::now();
00063 period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
00064 stopwatch_last = stopwatch_now;
00065
00066 while (ros::ok())
00067 {
00068
00069 hardware_interface.read(timestamp, period);
00070
00071
00072 timestamp = ros::Time::now();
00073 stopwatch_now = std::chrono::steady_clock::now();
00074 period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
00075 stopwatch_last = stopwatch_now;
00076
00077
00078 controller_manager.update(timestamp, period);
00079
00080
00081 hardware_interface.write(timestamp, period);
00082 }
00083
00084 spinner.stop();
00085 ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Shutting down.");
00086
00087 return 0;
00088 }