test_trajectory.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 <dave@dav.ee>
00036    Desc:   Generate a random trajectory to test the ros_control controller
00037 */
00038 
00039 // ROS
00040 #include <ros/ros.h>
00041 #include <control_msgs/FollowJointTrajectoryAction.h>
00042 #include <trajectory_msgs/JointTrajectory.h>
00043 #include <actionlib/client/simple_action_client.h>
00044 #include <actionlib/client/terminal_state.h>
00045 
00046 namespace ros_control_boilerplate
00047 {
00048 static const double SEC_PER_TRAJ_POINT = 5.0;  // time between points
00049 static const std::size_t TRAJ_POINTS = 10;     // number of points to generate
00050 
00051 class TestTrajectory
00052 {
00053 public:
00057   TestTrajectory()
00058     : nh_private_("~")
00059   {
00060     std::string action_topic;
00061     nh_private_.getParam("action_topic", action_topic);
00062     if (action_topic.empty())
00063     {
00064       ROS_FATAL_STREAM_NAMED(
00065           "test_trajectory",
00066           "Not follow joint trajectory action topic found on the parameter server");
00067       exit(-1);
00068     }
00069     ROS_INFO_STREAM_NAMED("test_trajectory", "Connecting to action " << action_topic);
00070 
00071     // create the action client
00072     // true causes the client to spin its own thread
00073     actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> action_client(
00074         action_topic, true);
00075 
00076     ROS_INFO_NAMED("test_trajetory", "Waiting for action server to start.");
00077     // wait for the action server to start
00078     action_client.waitForServer();  // will wait for infinite time
00079 
00080     ROS_INFO_NAMED("test_trajetory", "Action server started, sending goal.");
00081 
00082     // send a goal to the action
00083     control_msgs::FollowJointTrajectoryGoal goal;
00084     goal.trajectory = createTrajectory();
00085     std::cout << "Trajectry:\n" << goal.trajectory << std::endl;
00086     action_client.sendGoal(goal);
00087 
00088     // Wait for the action to return
00089     double wait_extra_padding = 2;  // time to wait longer than trajectory itself
00090     bool finished_before_timeout = action_client.waitForResult(
00091         ros::Duration(goal.trajectory.points.back().time_from_start.toSec() + wait_extra_padding));
00092 
00093     if (finished_before_timeout)
00094     {
00095       actionlib::SimpleClientGoalState state = action_client.getState();
00096       ROS_INFO_NAMED("test_trajetory", "Action finished: %s", state.toString().c_str());
00097     }
00098     else
00099       ROS_INFO_NAMED("test_trajetory", "Action did not finish before the time out.");
00100 
00101     ROS_INFO_STREAM_NAMED("test_trajectory", "TestTrajectory Finished");
00102   }
00103 
00107   trajectory_msgs::JointTrajectory createTrajectory()
00108   {
00109     std::vector<std::string> joint_names;
00110     double min_joint_value = -3.14;
00111     double max_joint_value = 3.14;
00112 
00113     // Get joint names
00114     nh_private_.getParam("hardware_interface/joints", joint_names);
00115     if (joint_names.size() == 0)
00116     {
00117       ROS_FATAL_STREAM_NAMED(
00118           "init",
00119           "Not joints found on parameter server for controller, did you load the proper yaml file?"
00120               << " Namespace: " << nh_private_.getNamespace() << "/hardware_interface/joints");
00121       exit(-1);
00122     }
00123 
00124     nh_private_.getParam("min_joint_value", min_joint_value);
00125     nh_private_.getParam("max_joint_value", max_joint_value);
00126     ROS_DEBUG_STREAM_NAMED("test_trajectory", "Creating trajectory with joint values from "
00127                                                   << min_joint_value << " to " << max_joint_value);
00128 
00129     // Create header
00130     trajectory_msgs::JointTrajectory trajectory;
00131     trajectory.header.stamp = ros::Time::now();
00132     trajectory.joint_names = joint_names;
00133 
00134     // Create trajectory with x points
00135     trajectory.points.resize(TRAJ_POINTS);
00136     for (std::size_t i = 0; i < TRAJ_POINTS; ++i)
00137     {
00138       trajectory.points[i].positions.resize(joint_names.size());
00139       // for each joint
00140       for (std::size_t j = 0; j < joint_names.size(); ++j)
00141       {
00142         trajectory.points[i].positions[j] = dRand(min_joint_value, max_joint_value);
00143         trajectory.points[i].time_from_start = ros::Duration(i * SEC_PER_TRAJ_POINT);
00144       }
00145     }
00146 
00147     return trajectory;
00148   }
00149 
00151   double dRand(double dMin, double dMax)
00152   {
00153     double d = (double)rand() / RAND_MAX;
00154     return dMin + d * (dMax - dMin);
00155   }
00156 
00157 private:
00158   // A shared node handle
00159   ros::NodeHandle nh_private_;
00160 
00161 };  // end class
00162 
00163 // Create boost pointers for this class
00164 typedef boost::shared_ptr<TestTrajectory> TestTrajectoryPtr;
00165 typedef boost::shared_ptr<const TestTrajectory> TestTrajectoryConstPtr;
00166 
00167 }  // end namespace
00168 
00169 int main(int argc, char** argv)
00170 {
00171   ros::init(argc, argv, "test_trajectory");
00172   ROS_INFO_STREAM_NAMED("test_trajectory", "Starting TestTrajectory...");
00173 
00174   // Allow the action server to recieve and send ros messages
00175   ros::AsyncSpinner spinner(2);
00176   spinner.start();
00177 
00178   ros_control_boilerplate::TestTrajectory server;
00179 
00180   ROS_INFO_STREAM_NAMED("test_trajectory", "Shutting down.");
00181   ros::shutdown();
00182 
00183   return 0;
00184 }


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