mrp2_hardware.cpp
Go to the documentation of this file.
00001 // ROS
00002 #include <ros/ros.h>
00003 
00004 // ros_control
00005 #include <controller_manager/controller_manager.h>
00006 
00007 #include "mrp2_hardware/mrp2_hardware.h"
00008 
00009 typedef boost::chrono::steady_clock time_source;
00010 
00011 static const double BILLION = 1000000000.0;
00012 
00013 int main(int argc, char **argv)
00014 {
00015 
00016         // Settings
00017         double cycle_time_error_threshold_ = 0.02;
00018 
00019         // Timing
00020         ros::Timer non_realtime_loop_;
00021         ros::Duration elapsed_time_;
00022         double loop_hz_ = 50;
00023         struct timespec last_time_;
00024         struct timespec current_time_;
00025         struct timespec read_time_, write_time_, update_time_;
00026         ros::Duration elapsed_time_read_, elapsed_time_write_, elapsed_time_update_;
00027 
00028         ros::init(argc, argv, "mrp2_hardware");
00029         ros::NodeHandle nh;
00030 
00031         MRP2HW robot;
00032  
00033         
00034 
00035         // Start timer
00036         ros::Duration desired_update_freq_ = ros::Duration(1 / loop_hz_);
00037 
00038         controller_manager::ControllerManager cm(&robot, nh);
00039 
00040         ros::Rate rate(loop_hz_);
00041         ros::AsyncSpinner spinner(1);
00042         spinner.start();
00043 
00044         // Get current time for use with first update
00045         clock_gettime(CLOCK_MONOTONIC, &last_time_);
00046         
00047         while(ros::ok())
00048         {
00049 
00050                 clock_gettime(CLOCK_MONOTONIC, &current_time_);
00051                 elapsed_time_ =
00052                   ros::Duration(current_time_.tv_sec - last_time_.tv_sec + (current_time_.tv_nsec - last_time_.tv_nsec) / BILLION);
00053 
00054                 elapsed_time_read_ = ros::Duration(read_time_.tv_sec - last_time_.tv_sec + (read_time_.tv_nsec - last_time_.tv_nsec) / BILLION);
00055                 elapsed_time_write_ = ros::Duration(write_time_.tv_sec - last_time_.tv_sec + (write_time_.tv_nsec - last_time_.tv_nsec) / BILLION);
00056                 elapsed_time_update_ = ros::Duration(update_time_.tv_sec - last_time_.tv_sec + (update_time_.tv_nsec - last_time_.tv_nsec) / BILLION);
00057 
00058                 last_time_ = current_time_;
00059                 const double cycle_time_error = (elapsed_time_ - desired_update_freq_).toSec();
00060                 if (cycle_time_error > cycle_time_error_threshold_)
00061                 {
00062                         ROS_WARN_STREAM("Cycle time exceeded error threshold by: "
00063                                                  << cycle_time_error << ", cycle time: " << elapsed_time_
00064                                                  << ", threshold: " << cycle_time_error_threshold_ << ", read time: " << elapsed_time_read_
00065                                                  << ", update time: " << elapsed_time_update_
00066                                                  << ", write time: " << elapsed_time_write_);
00067                 }
00068 
00069                 robot.read();
00070                 clock_gettime(CLOCK_MONOTONIC, &read_time_);
00071                 if(robot.estop_release)
00072         {
00073                 cm.update(ros::Time::now(), elapsed_time_, true);
00074                 ROS_WARN_STREAM("E-stop is released.");
00075                 robot.estop_release = false;
00076         }else{
00077                 cm.update(ros::Time::now(), elapsed_time_);
00078                         //ROS_WARN("robot time: %f", robot.getTime().toSec());
00079         }
00080         clock_gettime(CLOCK_MONOTONIC, &update_time_);
00081                 robot.write();
00082                 clock_gettime(CLOCK_MONOTONIC, &write_time_);
00083                 rate.sleep();
00084         }
00085         spinner.stop();
00086 
00087         return 0;
00088 }


mrp2_hardware
Author(s): Akif Hacinecipoglu
autogenerated on Thu Jun 6 2019 21:43:37