test_trajectory.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 <dave@dav.ee>
36  Desc: Generate a random trajectory to test the ros_control controller
37 */
38 
39 // ROS
40 #include <ros/ros.h>
41 #include <control_msgs/FollowJointTrajectoryAction.h>
42 #include <trajectory_msgs/JointTrajectory.h>
45 
47 {
48 static const double SEC_PER_TRAJ_POINT = 5.0; // time between points
49 static const std::size_t TRAJ_POINTS = 10; // number of points to generate
50 
52 {
53 public:
58  : nh_private_("~")
59  {
60  nh_private_.getParam("trajectory_controller", trajectory_controller);
61  if (trajectory_controller.empty())
62  {
64  "test_trajectory",
65  "No joint trajectory controller parameter found on the parameter server");
66  exit(-1);
67  }
68  ROS_INFO_STREAM_NAMED("test_trajectory",
69  "Connecting to controller " << trajectory_controller);
70 
71  // create the action client
72  // true causes the client to spin its own thread
74  trajectory_controller + "/follow_joint_trajectory/", true);
75 
76  ROS_INFO_NAMED("test_trajetory", "Waiting for action server to start.");
77  // wait for the action server to start
78  action_client.waitForServer(); // will wait for infinite time
79 
80  ROS_INFO_NAMED("test_trajetory", "Action server started, sending goal.");
81 
82  // send a goal to the action
83  control_msgs::FollowJointTrajectoryGoal goal;
84  goal.trajectory = createTrajectory();
85  std::cout << "Trajectry:\n" << goal.trajectory << std::endl;
86  action_client.sendGoal(goal);
87 
88  // Wait for the action to return
89  double wait_extra_padding = 2; // time to wait longer than trajectory itself
90  bool finished_before_timeout = action_client.waitForResult(
91  ros::Duration(goal.trajectory.points.back().time_from_start.toSec() + wait_extra_padding));
92 
93  if (finished_before_timeout)
94  {
95  actionlib::SimpleClientGoalState state = action_client.getState();
96  ROS_INFO_NAMED("test_trajetory", "Action finished: %s", state.toString().c_str());
97  }
98  else
99  ROS_INFO_NAMED("test_trajetory", "Action did not finish before the time out.");
100 
101  ROS_INFO_STREAM_NAMED("test_trajectory", "TestTrajectory Finished");
102  }
103 
107  trajectory_msgs::JointTrajectory createTrajectory()
108  {
109  std::vector<std::string> joint_names;
110  double min_joint_value = -3.14;
111  double max_joint_value = 3.14;
112 
113  // Get joint names
114  nh_private_.getParam(trajectory_controller + "/joints", joint_names);
115  if (joint_names.size() == 0)
116  {
118  "init",
119  "Not joints found on parameter server for controller, did you load the proper yaml file?"
120  << " Namespace: " << nh_private_.getNamespace() << "/hardware_interface/joints");
121  exit(-1);
122  }
123 
124  nh_private_.getParam("min_joint_value", min_joint_value);
125  nh_private_.getParam("max_joint_value", max_joint_value);
126  ROS_DEBUG_STREAM_NAMED("test_trajectory", "Creating trajectory with joint values from "
127  << min_joint_value << " to " << max_joint_value);
128 
129  // Create header
130  trajectory_msgs::JointTrajectory trajectory;
131  trajectory.header.stamp = ros::Time::now();
132  trajectory.joint_names = joint_names;
133 
134  // Create trajectory with x points
135  trajectory.points.resize(TRAJ_POINTS);
136  for (std::size_t i = 0; i < TRAJ_POINTS; ++i)
137  {
138  trajectory.points[i].positions.resize(joint_names.size());
139  // for each joint
140  for (std::size_t j = 0; j < joint_names.size(); ++j)
141  {
142  trajectory.points[i].positions[j] = dRand(min_joint_value, max_joint_value);
143  trajectory.points[i].time_from_start = ros::Duration(i * SEC_PER_TRAJ_POINT);
144  }
145  }
146 
147  return trajectory;
148  }
149 
151  double dRand(double dMin, double dMax)
152  {
153  double d = (double)rand() / RAND_MAX;
154  return dMin + d * (dMax - dMin);
155  }
156 
157 private:
158  // A shared node handle
160 
161  // A string containing the 'trajectory_controller' parameter value
163 }; // end class
164 
165 // Create boost pointers for this class
168 
169 } // end namespace
170 
171 int main(int argc, char** argv)
172 {
173  ros::init(argc, argv, "test_trajectory");
174  ROS_INFO_STREAM_NAMED("test_trajectory", "Starting TestTrajectory...");
175 
176  // Allow the action server to recieve and send ros messages
178  spinner.start();
179 
181 
182  ROS_INFO_STREAM_NAMED("test_trajectory", "Shutting down.");
183  ros::shutdown();
184 
185  return 0;
186 }
d
static const double SEC_PER_TRAJ_POINT
#define ROS_INFO_NAMED(name,...)
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
boost::shared_ptr< TestTrajectory > TestTrajectoryPtr
trajectory_msgs::JointTrajectory createTrajectory()
Create random trajectory.
#define ROS_DEBUG_STREAM_NAMED(name, args)
int main(int argc, char **argv)
double dRand(double dMin, double dMax)
Get random number.
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_INFO_STREAM_NAMED(name, args)
void spinner()
std::string toString() const
#define ROS_FATAL_STREAM_NAMED(name, args)
const std::string & getNamespace() const
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
boost::shared_ptr< const TestTrajectory > TestTrajectoryConstPtr
bool getParam(const std::string &key, std::string &s) const
static Time now()
ROSCPP_DECL void shutdown()
SimpleClientGoalState getState() const
static const std::size_t TRAJ_POINTS


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