csv_to_controller.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 #include <sstream>
45 #include <stdlib.h>
46 #include <iterator>
47 #include <vector>
48 #include <string>
49 
51 {
52 
53 CSVToController::CSVToController(const std::string& joint_trajectory_action,
54  const std::string& controller_state_topic)
55  : joint_trajectory_action_(joint_trajectory_action)
56  , controller_state_topic_(controller_state_topic)
57 {
58  ROS_INFO_STREAM_NAMED("csv_to_controller", "Waiting for action server");
60 
61  // State subscriber
62  state_sub_ = nh_.subscribe<control_msgs::JointTrajectoryControllerState>(
64 
65  // Wait for states to populate
66  ros::spinOnce();
67  ros::Duration(0.5).sleep();
68 
69  ROS_INFO_STREAM_NAMED("csv_to_controller", "CSVToController Ready.");
70 }
71 
73  const control_msgs::JointTrajectoryControllerState::ConstPtr& state)
74 {
75  current_state_ = *state;
76 }
77 
78 void CSVToController::printPoint(trajectory_msgs::JointTrajectoryPoint &point)
79 {
80  // Show new line
81  std::copy(point.positions.begin(), point.positions.end(), std::ostream_iterator<double>(std::cout, " "));
82  std::cout << std::endl;
83 }
84 
85 // Start the data collection
86 void CSVToController::loadAndRunCSV(const std::string& file_name)
87 {
88  file_name_ = file_name;
89 
90  // Open file
91  std::ifstream input_file;
92  input_file.open(file_name_.c_str());
93 
94  // Settings
95  std::string line;
96  std::string cell;
97 
98  control_msgs::FollowJointTrajectoryGoal pre_goal; // moving to start state
99  control_msgs::FollowJointTrajectoryGoal goal; // csv file
100 
101  // Populate joint names
102  goal.trajectory.joint_names.push_back("joint_a1");
103  goal.trajectory.joint_names.push_back("joint_a2");
104  goal.trajectory.joint_names.push_back("joint_a3");
105  goal.trajectory.joint_names.push_back("joint_a4");
106  goal.trajectory.joint_names.push_back("joint_a5");
107  goal.trajectory.joint_names.push_back("joint_a6");
108  goal.trajectory.joint_names.push_back("joint_a7");
109  pre_goal.trajectory.joint_names = goal.trajectory.joint_names;
110  double num_joints = goal.trajectory.joint_names.size();
111 
112  // Skip header
113  std::getline(input_file, line);
114 
115  // For each line/row
116  while (std::getline(input_file, line))
117  {
118  std::stringstream lineStream(line);
119 
120  trajectory_msgs::JointTrajectoryPoint point;
121 
122  // TIME FROM START
123  if (!std::getline(lineStream, cell, ','))
124  ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
125 
126  point.time_from_start = ros::Duration(atof(cell.c_str()));
127 
128  // For each item/column
129  for (std::size_t i = 0; i < num_joints; ++i)
130  {
131  // DESIRED POSITION
132  if (!std::getline(lineStream, cell, ','))
133  ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
134  // UNUSED
135 
136  // DESIRED VELOCITY
137  if (!std::getline(lineStream, cell, ','))
138  ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
139  // UNUSED
140 
141  // ACTUAL POSITION
142  if (!std::getline(lineStream, cell, ','))
143  ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
144  point.positions.push_back(atof(cell.c_str()));
145 
146  // ACTUAL VELOCITY
147  if (!std::getline(lineStream, cell, ','))
148  ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
149  point.velocities.push_back(atof(cell.c_str()));
150 
151  // COMMANDED VEL
152  if (!std::getline(lineStream, cell, ','))
153  ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
154  // UNUSED
155  }
156 
157  goal.trajectory.points.push_back(point);
158  } // while
159 
160  // Check that we have a current state
161  if (current_state_.actual.positions.empty())
162  {
163  ROS_ERROR_STREAM_NAMED("csv_to_controller","Unable to find current state msg");
164  return;
165  }
166  // Add current state to start of trajectory
167  trajectory_msgs::JointTrajectoryPoint last_point;
168  last_point.positions = current_state_.actual.positions;
169  last_point.velocities = current_state_.actual.velocities;
170 
171  std::cout << "Current State:" << std::endl;
172  printPoint(last_point);
173  printPoint(goal.trajectory.points.front());
174  std::cout << "^^ Goal point " << std::endl;
175  pre_goal.trajectory.points.push_back(last_point);
176 
177  // Interpolate from first point
178  bool done = false;
179  double max_velocity = 0.1; // m/s or radians/s
180  double frequency = 200; // hz
181  double q_delta = max_velocity / frequency; // m
182  double t_delta = 1/frequency;
183  ros::Duration time_from_start(1);
184  while(!done)
185  {
186  done = true;
187  trajectory_msgs::JointTrajectoryPoint new_point = last_point;
188 
189  // Time change
190  time_from_start += ros::Duration(t_delta);
191  new_point.time_from_start = time_from_start;
192 
193  // Position change
194  for (std::size_t i = 0; i < num_joints; ++i) // each joint
195  {
196  // Do we need to move this joint foward?
197  if (new_point.positions[i] < goal.trajectory.points.front().positions[i])
198  {
199  // Do not allow to go past goal point
200  new_point.positions[i] = std::min(new_point.positions[i] + q_delta,
201  goal.trajectory.points.front().positions[i]);
202  new_point.velocities[i] = max_velocity;
203  done = false;
204  } else {
205  // Maintain velocity
206  new_point.velocities[i] = 0;
207  }
208  }
209  pre_goal.trajectory.points.push_back(new_point);
210  last_point = new_point;
211 
212  printPoint(new_point);
213  }
214 
215  //std::cout << "TRAJECTORY: " << trajectory << std::endl;
216  //std::cout << "TRAJECTORY: " << goal.trajectory << std::endl;
217 
218  ROS_INFO_STREAM_NAMED("temp","Sleeping for " << time_from_start.toSec());
219  pre_goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.5);
221  ros::Duration(time_from_start).sleep();
222 
223  for (std::size_t i = 0; i < goal.trajectory.points.size(); ++i)
224  {
225  printPoint(goal.trajectory.points[i]);
226  }
227 
228  ROS_INFO_STREAM_NAMED("csv_to_controller","Preparing to follow CSV path");
229  ros::Duration(0.5).sleep();
230  goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.5);
232 }
233 
234 } // end namespace
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
void stateCB(const control_msgs::JointTrajectoryControllerState::ConstPtr &state)
Callback from ROS message.
#define ROS_ERROR_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())
bool sleep() const
#define ROS_INFO_STREAM_NAMED(name, args)
control_msgs::JointTrajectoryControllerState current_state_
CSVToController(const std::string &joint_trajectory_action, const std::string &controller_state_topic)
Constructor.
void printPoint(trajectory_msgs::JointTrajectoryPoint &point)
actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > joint_trajectory_action_
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
static Time now()
ROSCPP_DECL void spinOnce()
void loadAndRunCSV(const std::string &file_name)


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