csv_to_controller.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/csv_to_controller.h>
00040 
00041 // Basic file operations
00042 #include <iostream>
00043 #include <fstream>
00044 #include <sstream>
00045 #include <stdlib.h>
00046 #include <iterator>
00047 #include <vector>
00048 #include <string>
00049 
00050 namespace ros_control_boilerplate
00051 {
00052 
00053 CSVToController::CSVToController(const std::string& joint_trajectory_action,
00054                                  const std::string& controller_state_topic)
00055   : joint_trajectory_action_(joint_trajectory_action)
00056   , controller_state_topic_(controller_state_topic)
00057 {
00058   ROS_INFO_STREAM_NAMED("csv_to_controller", "Waiting for action server");
00059   joint_trajectory_action_.waitForServer();
00060 
00061   // State subscriber
00062   state_sub_ = nh_.subscribe<control_msgs::JointTrajectoryControllerState>(
00063       controller_state_topic_, 1, &CSVToController::stateCB, this);
00064 
00065   // Wait for states to populate
00066   ros::spinOnce();
00067   ros::Duration(0.5).sleep();
00068 
00069   ROS_INFO_STREAM_NAMED("csv_to_controller", "CSVToController Ready.");
00070 }
00071 
00072 void CSVToController::stateCB(
00073     const control_msgs::JointTrajectoryControllerState::ConstPtr& state)
00074 {
00075   current_state_ = *state;
00076 }
00077 
00078 void CSVToController::printPoint(trajectory_msgs::JointTrajectoryPoint &point)
00079 {
00080     // Show new line
00081     std::copy(point.positions.begin(), point.positions.end(), std::ostream_iterator<double>(std::cout, " "));
00082     std::cout << std::endl;
00083 }
00084 
00085 // Start the data collection
00086 void CSVToController::loadAndRunCSV(const std::string& file_name)
00087 {
00088   file_name_ = file_name;
00089 
00090   // Open file
00091   std::ifstream input_file;
00092   input_file.open(file_name_.c_str());
00093 
00094   // Settings
00095   std::string line;
00096   std::string cell;
00097 
00098   control_msgs::FollowJointTrajectoryGoal pre_goal; // moving to start state
00099   control_msgs::FollowJointTrajectoryGoal goal; // csv file
00100 
00101   // Populate joint names
00102   goal.trajectory.joint_names.push_back("joint_a1");
00103   goal.trajectory.joint_names.push_back("joint_a2");
00104   goal.trajectory.joint_names.push_back("joint_a3");
00105   goal.trajectory.joint_names.push_back("joint_a4");
00106   goal.trajectory.joint_names.push_back("joint_a5");
00107   goal.trajectory.joint_names.push_back("joint_a6");
00108   goal.trajectory.joint_names.push_back("joint_a7");
00109   pre_goal.trajectory.joint_names = goal.trajectory.joint_names;
00110   double num_joints = goal.trajectory.joint_names.size();
00111 
00112   // Skip header
00113   std::getline(input_file, line);
00114 
00115   // For each line/row
00116   while (std::getline(input_file, line))
00117   {
00118     std::stringstream lineStream(line);
00119 
00120     trajectory_msgs::JointTrajectoryPoint point;
00121 
00122     // TIME FROM START
00123     if (!std::getline(lineStream, cell, ','))
00124       ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
00125 
00126     point.time_from_start = ros::Duration(atof(cell.c_str()));
00127 
00128     // For each item/column
00129     for (std::size_t i = 0; i < num_joints; ++i)
00130     {
00131       // DESIRED POSITION
00132       if (!std::getline(lineStream, cell, ','))
00133         ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
00134       // UNUSED
00135 
00136       // DESIRED VELOCITY
00137       if (!std::getline(lineStream, cell, ','))
00138         ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
00139       // UNUSED
00140 
00141       // ACTUAL POSITION
00142       if (!std::getline(lineStream, cell, ','))
00143         ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
00144       point.positions.push_back(atof(cell.c_str()));
00145 
00146       // ACTUAL VELOCITY
00147       if (!std::getline(lineStream, cell, ','))
00148         ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
00149       point.velocities.push_back(atof(cell.c_str()));
00150 
00151       // COMMANDED VEL
00152       if (!std::getline(lineStream, cell, ','))
00153         ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
00154       // UNUSED
00155     }
00156 
00157     goal.trajectory.points.push_back(point);
00158   }  // while
00159 
00160   // Check that we have a current state
00161   if (current_state_.actual.positions.empty())
00162   {
00163     ROS_ERROR_STREAM_NAMED("csv_to_controller","Unable to find current state msg");
00164     return;
00165   }
00166   // Add current state to start of trajectory
00167   trajectory_msgs::JointTrajectoryPoint last_point;
00168   last_point.positions = current_state_.actual.positions;
00169   last_point.velocities = current_state_.actual.velocities;
00170 
00171   std::cout << "Current State:" << std::endl;
00172   printPoint(last_point);
00173   printPoint(goal.trajectory.points.front());
00174   std::cout << "^^ Goal point " << std::endl;
00175   pre_goal.trajectory.points.push_back(last_point);
00176 
00177   // Interpolate from first point
00178   bool done = false;
00179   double max_velocity = 0.1; // m/s  or radians/s
00180   double frequency = 200; // hz
00181   double q_delta = max_velocity / frequency; // m
00182   double t_delta = 1/frequency;
00183   ros::Duration time_from_start(1);
00184   while(!done)
00185   {
00186     done = true;
00187     trajectory_msgs::JointTrajectoryPoint new_point = last_point;
00188 
00189     // Time change
00190     time_from_start += ros::Duration(t_delta);
00191     new_point.time_from_start = time_from_start;
00192 
00193     // Position change
00194     for (std::size_t i = 0; i < num_joints; ++i) // each joint
00195     {
00196       // Do we need to move this joint foward?
00197       if (new_point.positions[i] < goal.trajectory.points.front().positions[i])
00198       {
00199         // Do not allow to go past goal point
00200         new_point.positions[i] = std::min(new_point.positions[i] + q_delta,
00201                                           goal.trajectory.points.front().positions[i]);
00202         new_point.velocities[i] = max_velocity;
00203         done = false;
00204       } else {
00205         // Maintain velocity
00206         new_point.velocities[i] = 0;
00207       }
00208     }
00209     pre_goal.trajectory.points.push_back(new_point);
00210     last_point = new_point;
00211 
00212     printPoint(new_point);
00213   }
00214 
00215   //std::cout << "TRAJECTORY: " << trajectory << std::endl;
00216   //std::cout << "TRAJECTORY: " << goal.trajectory << std::endl;
00217 
00218   ROS_INFO_STREAM_NAMED("temp","Sleeping for " << time_from_start.toSec());
00219   pre_goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.5);
00220   joint_trajectory_action_.sendGoal(pre_goal);
00221   ros::Duration(time_from_start).sleep();
00222 
00223   for (std::size_t i = 0; i < goal.trajectory.points.size(); ++i)
00224   {
00225     printPoint(goal.trajectory.points[i]);
00226   }
00227 
00228   ROS_INFO_STREAM_NAMED("csv_to_controller","Preparing to follow CSV path");
00229   ros::Duration(0.5).sleep();
00230   goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.5);
00231   joint_trajectory_action_.sendGoal(goal);
00232 }
00233 
00234 } // end namespace


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