controller_to_csv.cpp
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 #include <ros_control_boilerplate/tools/controller_to_csv.h>
00040 
00041 // Basic file operations
00042 #include <iostream>
00043 #include <fstream>
00044 
00045 // ROS parameter loading
00046 #include <rosparam_shortcuts/rosparam_shortcuts.h>
00047 
00048 namespace ros_control_boilerplate
00049 {
00050 
00051 ControllerToCSV::ControllerToCSV(const std::string& topic)
00052   : nh_("~")
00053   , first_update_(true)
00054   , recording_started_(true)
00055 {
00056   // Load rosparams
00057   ros::NodeHandle rpsnh(nh_, name_);
00058   int error = 0;
00059   error += !rosparam_shortcuts::get(name_, rpsnh, "record_hz", record_hz_);
00060   rosparam_shortcuts::shutdownIfError(name_, error);
00061 
00062   ROS_INFO_STREAM_NAMED(name_, "Subscribing to " << topic);
00063   // State subscriber
00064   state_sub_ = nh_.subscribe<control_msgs::JointTrajectoryControllerState>(
00065       topic, 1, &ControllerToCSV::stateCB, this);
00066 
00067   // Wait for states to populate
00068   waitForSubscriber(state_sub_);
00069 
00070   // Alert user to mode
00071   if (recordAll())
00072   {
00073     ROS_INFO_STREAM_NAMED(name_, "Recording all incoming controller state messages");
00074   }
00075   else
00076   {
00077     ROS_INFO_STREAM_NAMED(name_, "Only recording every " << record_hz_ << " hz");
00078   }
00079 
00080   ROS_INFO_STREAM_NAMED(name_, "ControllerToCSV Ready.");
00081 }
00082 
00083 ControllerToCSV::~ControllerToCSV()
00084 {
00085   stopRecording();
00086 }
00087 
00088 bool ControllerToCSV::recordAll()
00089 {
00090   return record_hz_ == 0;
00091 }
00092 
00093 void ControllerToCSV::stateCB(const control_msgs::JointTrajectoryControllerState::ConstPtr& state)
00094 {
00095   // Two modes - save all immediately, or only save at certain frequency
00096   if (recordAll())
00097   {
00098     // Record state
00099     states_.push_back(current_state_);
00100 
00101     // Record current time
00102     timestamps_.push_back(ros::Time::now());
00103   }
00104   else // only record at freq
00105   {
00106     current_state_ = *state;
00107   }
00108 }
00109 
00110 // Start the data collection
00111 void ControllerToCSV::startRecording(const std::string& file_name)
00112 {
00113   ROS_INFO_STREAM_NAMED(name_, "Saving to " << file_name);
00114   file_name_ = file_name;
00115 
00116   // Reset data collections
00117   states_.clear();
00118   timestamps_.clear();
00119 
00120   recording_started_ = true;
00121 
00122   // Start sampling loop
00123   if (!recordAll()) // only record at the specified frequency
00124   {
00125     ros::Duration update_freq = ros::Duration(1.0 / record_hz_);
00126     non_realtime_loop_ = nh_.createTimer(update_freq, &ControllerToCSV::update, this);
00127   }
00128 }
00129 
00130 void ControllerToCSV::update(const ros::TimerEvent& event)
00131 {
00132   if (first_update_)
00133   {
00134     // Check if we've recieved any states yet
00135     if (current_state_.joint_names.empty())
00136     {
00137       ROS_WARN_STREAM_THROTTLE_NAMED(2, name_, "No states recieved yet");
00138       return;
00139     }
00140     first_update_ = false;
00141   }
00142   else // if (event.last_real > 0)
00143   {
00144     const double freq = 1.0 / (event.current_real - event.last_real).toSec();
00145     ROS_INFO_STREAM_THROTTLE_NAMED(2, name_, "Updating at " << freq << " hz, total: " << (ros::Time::now() - timestamps_.front()).toSec() << " seconds");
00146   }
00147   // Record state
00148   states_.push_back(current_state_);
00149 
00150   // Record current time
00151   timestamps_.push_back(ros::Time::now());
00152 }
00153 
00154 void ControllerToCSV::stopRecording()
00155 {
00156   non_realtime_loop_.stop();
00157   writeToFile();
00158 }
00159 
00160 bool ControllerToCSV::writeToFile()
00161 {
00162   std::cout << "Writing data to file " << std::endl;
00163 
00164   if (!states_.size())
00165   {
00166     std::cout << "No controller states populated" << std::endl;
00167     return false;
00168   }
00169 
00170   std::ofstream output_file;
00171   output_file.open(file_name_.c_str());
00172 
00173   // Output header -------------------------------------------------------
00174   output_file << "timestamp,";
00175   for (std::size_t j = 0; j < states_[0].joint_names.size(); ++j)
00176   {
00177     output_file << states_[0].joint_names[j] << "_desired_pos,"
00178                 << states_[0].joint_names[j] << "_desired_vel,"
00179                 << states_[0].joint_names[j] << "_actual_pos,"
00180                 << states_[0].joint_names[j] << "_actual_vel,"
00181                 << states_[0].joint_names[j] << "_error_pos,"
00182                 << states_[0].joint_names[j] << "_error_vel,";
00183   }
00184   output_file << std::endl;
00185 
00186   // Output data ------------------------------------------------------
00187 
00188   // Subtract starting time
00189   // double start_time = states_[0].header.stamp.toSec();
00190   double start_time = timestamps_[0].toSec();
00191 
00192   for (std::size_t i = 0; i < states_.size(); ++i)
00193   {
00194     // Timestamp
00195     output_file << timestamps_[i].toSec() - start_time << ",";
00196 
00197     // Output entire state of robot to single line
00198     for (std::size_t j = 0; j < states_[i].joint_names.size(); ++j)
00199     {
00200       // Output State
00201       output_file << states_[i].desired.positions[j] << ","
00202                   << states_[i].desired.velocities[j] << ","
00203                   << states_[i].actual.positions[j] << ","
00204                   << states_[i].actual.velocities[j] << ","
00205                   << states_[i].error.positions[j] << ","
00206                   << states_[i].error.velocities[j] << ",";
00207     }
00208 
00209     output_file << std::endl;
00210   }
00211   output_file.close();
00212   std::cout << "Wrote to file: " << file_name_ << std::endl;
00213   return true;
00214 }
00215 
00216 bool ControllerToCSV::waitForSubscriber(const ros::Subscriber& sub, const double& wait_time)
00217 {
00218   // Benchmark runtime
00219   ros::Time start_time;
00220   start_time = ros::Time::now();
00221 
00222   // Will wait at most 1000 ms (1 sec)
00223   ros::Time max_time(ros::Time::now() + ros::Duration(wait_time));
00224 
00225   // This is wrong. It returns only the number of subscribers that have already established their
00226   // direct connections to this subscriber
00227   int num_existing_subscribers = sub.getNumPublishers();
00228 
00229   // How often to check for subscribers
00230   ros::Rate poll_rate(200);
00231 
00232   // Wait for subsriber
00233   while (num_existing_subscribers == 0)
00234   {
00235     // Check if timed out
00236     if (ros::Time::now() > max_time)
00237     {
00238       ROS_WARN_STREAM_NAMED(name_, "Topic '" << sub.getTopic() << "' unable to connect to any publishers within "
00239                                                       << wait_time << " seconds.");
00240       return false;
00241     }
00242     ros::spinOnce();
00243 
00244     // Sleep
00245     poll_rate.sleep();
00246 
00247     // Check again
00248     num_existing_subscribers = sub.getNumPublishers();
00249   }
00250 
00251   // Benchmark runtime
00252   if (true)
00253   {
00254     double duration = (ros::Time::now() - start_time).toSec();
00255     ROS_DEBUG_STREAM_NAMED(name_, "Topic '" << sub.getTopic() << "' took " << duration
00256                                                      << " seconds to connect to a subscriber. "
00257                                                         "Connected to " << num_existing_subscribers
00258                                                      << " total subsribers");
00259   }
00260   return true;
00261 }
00262 
00263 }  // end namespace


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