controller_to_csv.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, University of Colorado, Boulder
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Univ of CO, Boulder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman
36  Desc: Records a ros_control ControllerState data to CSV for Matlab/etc analysis
37 */
38 
40 
41 // Basic file operations
42 #include <iostream>
43 #include <fstream>
44 
45 // ROS parameter loading
47 
49 {
50 
51 ControllerToCSV::ControllerToCSV(const std::string& topic)
52  : nh_("~")
53  , first_update_(true)
54  , recording_started_(true)
55 {
56  // Load rosparams
57  ros::NodeHandle rpsnh(nh_, name_);
58  int error = 0;
59  error += !rosparam_shortcuts::get(name_, rpsnh, "record_hz", record_hz_);
61 
62  ROS_INFO_STREAM_NAMED(name_, "Subscribing to " << topic);
63  // State subscriber
64  state_sub_ = nh_.subscribe<control_msgs::JointTrajectoryControllerState>(
65  topic, 1, &ControllerToCSV::stateCB, this);
66 
67  // Wait for states to populate
69 
70  // Alert user to mode
71  if (recordAll())
72  {
73  ROS_INFO_STREAM_NAMED(name_, "Recording all incoming controller state messages");
74  }
75  else
76  {
77  ROS_INFO_STREAM_NAMED(name_, "Only recording every " << record_hz_ << " hz");
78  }
79 
80  ROS_INFO_STREAM_NAMED(name_, "ControllerToCSV Ready.");
81 }
82 
84 {
85  stopRecording();
86 }
87 
89 {
90  return record_hz_ == 0;
91 }
92 
93 void ControllerToCSV::stateCB(const control_msgs::JointTrajectoryControllerState::ConstPtr& state)
94 {
95  // Two modes - save all immediately, or only save at certain frequency
96  if (recordAll())
97  {
98  // Record state
99  states_.push_back(current_state_);
100 
101  // Record current time
102  timestamps_.push_back(ros::Time::now());
103  }
104  else // only record at freq
105  {
106  current_state_ = *state;
107  }
108 }
109 
110 // Start the data collection
111 void ControllerToCSV::startRecording(const std::string& file_name)
112 {
113  ROS_INFO_STREAM_NAMED(name_, "Saving to " << file_name);
114  file_name_ = file_name;
115 
116  // Reset data collections
117  states_.clear();
118  timestamps_.clear();
119 
120  recording_started_ = true;
121 
122  // Start sampling loop
123  if (!recordAll()) // only record at the specified frequency
124  {
125  ros::Duration update_freq = ros::Duration(1.0 / record_hz_);
127  }
128 }
129 
131 {
132  if (first_update_)
133  {
134  // Check if we've recieved any states yet
135  if (current_state_.joint_names.empty())
136  {
137  ROS_WARN_STREAM_THROTTLE_NAMED(2, name_, "No states recieved yet");
138  return;
139  }
140  first_update_ = false;
141  }
142  else // if (event.last_real > 0)
143  {
144  const double freq = 1.0 / (event.current_real - event.last_real).toSec();
145  ROS_INFO_STREAM_THROTTLE_NAMED(2, name_, "Updating at " << freq << " hz, total: " << (ros::Time::now() - timestamps_.front()).toSec() << " seconds");
146  }
147  // Record state
148  states_.push_back(current_state_);
149 
150  // Record current time
151  timestamps_.push_back(ros::Time::now());
152 }
153 
155 {
157  writeToFile();
158 }
159 
161 {
162  std::cout << "Writing data to file " << std::endl;
163 
164  if (!states_.size())
165  {
166  std::cout << "No controller states populated" << std::endl;
167  return false;
168  }
169 
170  std::ofstream output_file;
171  output_file.open(file_name_.c_str());
172 
173  // Output header -------------------------------------------------------
174  output_file << "timestamp,";
175  for (std::size_t j = 0; j < states_[0].joint_names.size(); ++j)
176  {
177  output_file << states_[0].joint_names[j] << "_desired_pos,"
178  << states_[0].joint_names[j] << "_desired_vel,"
179  << states_[0].joint_names[j] << "_actual_pos,"
180  << states_[0].joint_names[j] << "_actual_vel,"
181  << states_[0].joint_names[j] << "_error_pos,"
182  << states_[0].joint_names[j] << "_error_vel,";
183  }
184  output_file << std::endl;
185 
186  // Output data ------------------------------------------------------
187 
188  // Subtract starting time
189  // double start_time = states_[0].header.stamp.toSec();
190  double start_time = timestamps_[0].toSec();
191 
192  for (std::size_t i = 0; i < states_.size(); ++i)
193  {
194  // Timestamp
195  output_file << timestamps_[i].toSec() - start_time << ",";
196 
197  // Output entire state of robot to single line
198  for (std::size_t j = 0; j < states_[i].joint_names.size(); ++j)
199  {
200  // Output State
201  output_file << states_[i].desired.positions[j] << ","
202  << states_[i].desired.velocities[j] << ","
203  << states_[i].actual.positions[j] << ","
204  << states_[i].actual.velocities[j] << ","
205  << states_[i].error.positions[j] << ","
206  << states_[i].error.velocities[j] << ",";
207  }
208 
209  output_file << std::endl;
210  }
211  output_file.close();
212  std::cout << "Wrote to file: " << file_name_ << std::endl;
213  return true;
214 }
215 
216 bool ControllerToCSV::waitForSubscriber(const ros::Subscriber& sub, const double& wait_time)
217 {
218  // Benchmark runtime
219  ros::Time start_time;
220  start_time = ros::Time::now();
221 
222  // Will wait at most 1000 ms (1 sec)
223  ros::Time max_time(ros::Time::now() + ros::Duration(wait_time));
224 
225  // This is wrong. It returns only the number of subscribers that have already established their
226  // direct connections to this subscriber
227  int num_existing_subscribers = sub.getNumPublishers();
228 
229  // How often to check for subscribers
230  ros::Rate poll_rate(200);
231 
232  // Wait for subsriber
233  while (num_existing_subscribers == 0)
234  {
235  // Check if timed out
236  if (ros::Time::now() > max_time)
237  {
238  ROS_WARN_STREAM_NAMED(name_, "Topic '" << sub.getTopic() << "' unable to connect to any publishers within "
239  << wait_time << " seconds.");
240  return false;
241  }
242  ros::spinOnce();
243 
244  // Sleep
245  poll_rate.sleep();
246 
247  // Check again
248  num_existing_subscribers = sub.getNumPublishers();
249  }
250 
251  // Benchmark runtime
252  if (true)
253  {
254  double duration = (ros::Time::now() - start_time).toSec();
255  ROS_DEBUG_STREAM_NAMED(name_, "Topic '" << sub.getTopic() << "' took " << duration
256  << " seconds to connect to a subscriber. "
257  "Connected to " << num_existing_subscribers
258  << " total subsribers");
259  }
260  return true;
261 }
262 
263 } // end namespace
#define ROS_DEBUG_STREAM_NAMED(name, args)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void stop()
bool writeToFile()
Send all resulting data to file.
#define ROS_INFO_STREAM_NAMED(name, args)
void stateCB(const control_msgs::JointTrajectoryControllerState::ConstPtr &state)
Callback from ROS message.
void update(const ros::TimerEvent &e)
Recieve data from controller via ROS message.
bool recordAll()
Whether to record at a specific frequency, or record all incoming data.
control_msgs::JointTrajectoryControllerState current_state_
#define ROS_WARN_STREAM_THROTTLE_NAMED(rate, name, args)
uint32_t getNumPublishers() const
std::string getTopic() const
std::vector< control_msgs::JointTrajectoryControllerState > states_
bool waitForSubscriber(const ros::Subscriber &sub, const double &wait_time=10.0)
Check if topic has been connected to successfully.
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
bool sleep()
void shutdownIfError(const std::string &parent_name, std::size_t error_count)
static Time now()
void startRecording(const std::string &file_name)
Start the data collection.
ROSCPP_DECL void spinOnce()
bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, bool &value)
ControllerToCSV(const std::string &topic)
Constructor.
#define ROS_INFO_STREAM_THROTTLE_NAMED(rate, name, args)
#define ROS_WARN_STREAM_NAMED(name, args)


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Thu Feb 25 2021 03:58:54