41 #include <control_msgs/FollowJointTrajectoryAction.h> 42 #include <trajectory_msgs/JointTrajectory.h> 65 "No joint trajectory controller parameter found on the parameter server");
76 ROS_INFO_NAMED(
"test_trajetory",
"Waiting for action server to start.");
80 ROS_INFO_NAMED(
"test_trajetory",
"Action server started, sending goal.");
83 control_msgs::FollowJointTrajectoryGoal goal;
85 std::cout <<
"Trajectry:\n" << goal.trajectory << std::endl;
89 double wait_extra_padding = 2;
91 ros::Duration(goal.trajectory.points.back().time_from_start.toSec() + wait_extra_padding));
93 if (finished_before_timeout)
99 ROS_INFO_NAMED(
"test_trajetory",
"Action did not finish before the time out.");
109 std::vector<std::string> joint_names;
110 double min_joint_value = -3.14;
111 double max_joint_value = 3.14;
115 if (joint_names.size() == 0)
119 "Not joints found on parameter server for controller, did you load the proper yaml file?" 127 << min_joint_value <<
" to " << max_joint_value);
130 trajectory_msgs::JointTrajectory trajectory;
132 trajectory.joint_names = joint_names;
135 trajectory.points.resize(TRAJ_POINTS);
138 trajectory.points[i].positions.resize(joint_names.size());
140 for (std::size_t j = 0; j < joint_names.size(); ++j)
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);
151 double dRand(
double dMin,
double dMax)
153 double d = (double)rand() / RAND_MAX;
154 return dMin + d * (dMax - dMin);
171 int main(
int argc,
char** argv)
173 ros::init(argc, argv,
"test_trajectory");
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)
std::string toString() const
TestTrajectory()
Constructor.
std::string trajectory_controller
#define ROS_FATAL_STREAM_NAMED(name, args)
ros::NodeHandle nh_private_
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
ROSCPP_DECL void shutdown()
SimpleClientGoalState getState() const
static const std::size_t TRAJ_POINTS