controller_to_csv.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, University of Colorado, Boulder
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman
00036    Desc:   Records a ros_control ControllerState data to CSV for Matlab/etc analysis
00037 */
00038 
00039 #ifndef GENERIC_ROS_CONTROL__CONTROLLER_TO_CSV_H
00040 #define GENERIC_ROS_CONTROL__CONTROLLER_TO_CSV_H
00041 
00042 // ROS
00043 #include <ros/ros.h>
00044 
00045 // ros_control
00046 #include <control_msgs/JointTrajectoryControllerState.h>
00047 
00048 namespace ros_control_boilerplate
00049 {
00050 
00051 class ControllerToCSV
00052 {
00053 public:
00058   ControllerToCSV(const std::string& topic);
00059 
00061   ~ControllerToCSV();
00062 
00064   bool recordAll();
00065 
00067   void startRecording(const std::string& file_name);
00068 
00070   void stopRecording();
00071 
00072 private:
00073 
00075   bool writeToFile();
00076 
00078   void stateCB(const control_msgs::JointTrajectoryControllerState::ConstPtr& state);
00079 
00081   void update(const ros::TimerEvent& e);
00082 
00084   bool waitForSubscriber(const ros::Subscriber &sub, const double &wait_time = 10.0);
00085 
00086   // Class name
00087   std::string name_ = "controller_to_csv";
00088 
00089   // A shared node handle
00090   ros::NodeHandle nh_;
00091 
00092   // Show status info on first update
00093   bool first_update_;
00094   bool recording_started_;
00095 
00096   // Listener to state of controller
00097   ros::Subscriber state_sub_;
00098   double record_hz_; // how often to record the latest incoming data. if zero, record all
00099 
00100   // Where to save the CSV
00101   std::string file_name_;
00102 
00103   // Buffer of controller state data
00104   std::vector<control_msgs::JointTrajectoryControllerState> states_;
00105   std::vector<ros::Time> timestamps_;
00106 
00107   // Cache of last recieved state
00108   control_msgs::JointTrajectoryControllerState current_state_;
00109 
00110   // How often to sample the state
00111   ros::Timer non_realtime_loop_;
00112 
00113 };  // end class
00114 
00115 // Create boost pointers for this class
00116 typedef boost::shared_ptr<ControllerToCSV> ControllerToCSVPtr;
00117 typedef boost::shared_ptr<const ControllerToCSV> ControllerToCSVConstPtr;
00118 
00119 }  // end namespace
00120 
00121 #endif


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Sun Apr 24 2016 04:39:29