54 , recording_started_(true)
144 const double freq = 1.0 / (
event.current_real -
event.last_real).toSec();
162 std::cout <<
"Writing data to file " << std::endl;
166 std::cout <<
"No controller states populated" << std::endl;
170 std::ofstream output_file;
174 output_file <<
"timestamp,";
175 for (std::size_t j = 0; j <
states_[0].joint_names.size(); ++j)
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,";
184 output_file << std::endl;
192 for (std::size_t i = 0; i <
states_.size(); ++i)
195 output_file <<
timestamps_[i].toSec() - start_time <<
",";
198 for (std::size_t j = 0; j <
states_[i].joint_names.size(); ++j)
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] <<
",";
209 output_file << std::endl;
212 std::cout <<
"Wrote to file: " <<
file_name_ << std::endl;
233 while (num_existing_subscribers == 0)
239 << wait_time <<
" seconds.");
256 <<
" seconds to connect to a subscriber. " 257 "Connected to " << num_existing_subscribers
258 <<
" total subsribers");
#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())
~ControllerToCSV()
Destructor.
void stopRecording()
End recording.
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_
ros::Subscriber state_sub_
#define ROS_WARN_STREAM_THROTTLE_NAMED(rate, name, args)
std::vector< ros::Time > timestamps_
uint32_t getNumPublishers() const
ros::Timer non_realtime_loop_
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
void shutdownIfError(const std::string &parent_name, std::size_t error_count)
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 ¶m_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)