joint_trajectory.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019, the ypspur_ros authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <ros/ros.h>
31 
32 #include <rosgraph_msgs/Clock.h>
33 #include <sensor_msgs/JointState.h>
34 #include <trajectory_msgs/JointTrajectory.h>
35 
36 #include <gtest/gtest.h>
37 
38 TEST(JointTrajectory, CommandValidation)
39 {
40  ros::WallDuration wait(0.005);
41  ros::Duration clock_step(0.05);
42 
43  ros::NodeHandle nh;
44  ros::Publisher pub_cmd =
45  nh.advertise<trajectory_msgs::JointTrajectory>("joint_trajectory", 1, true);
46  ros::Publisher pub_clock =
47  nh.advertise<rosgraph_msgs::Clock>("clock", 100);
48 
49  sensor_msgs::JointState::ConstPtr joint_states;
50  const boost::function<void(const sensor_msgs::JointState::ConstPtr &)> cb_joint =
51  [&joint_states](const sensor_msgs::JointState::ConstPtr &msg) -> void
52  {
53  joint_states = msg;
54  };
55  ros::Subscriber sub_joint_states =
56  nh.subscribe("joint_states", 100, cb_joint);
57 
58  rosgraph_msgs::Clock clock;
59  clock.clock.fromNSec(ros::WallTime::now().toNSec());
60  pub_clock.publish(clock);
61 
62  // Wait until ypspur_ros
63  for (int i = 0; i < 1000; ++i)
64  {
65  wait.sleep();
66  ros::spinOnce();
67  if (joint_states)
68  break;
69  }
70 
71  // Publish valid command
72  trajectory_msgs::JointTrajectory cmd;
73  cmd.header.stamp = clock.clock;
74  cmd.joint_names.resize(1);
75  cmd.joint_names[0] = "joint0";
76  cmd.points.resize(1);
77  cmd.points[0].time_from_start = ros::Duration(1);
78  cmd.points[0].positions.resize(1);
79  cmd.points[0].positions[0] = 1.0;
80  cmd.points[0].velocities.resize(1);
81  cmd.points[0].velocities[0] = 1.0;
82  pub_cmd.publish(cmd);
83  wait.sleep();
84 
85  for (int i = 0; i < 50; ++i)
86  {
87  clock.clock += clock_step;
88  pub_clock.publish(clock);
89  wait.sleep();
90  ros::spinOnce();
91  }
92 
93  // Valid command must not be ignored
94  ASSERT_TRUE(static_cast<bool>(joint_states));
95  ASSERT_EQ(joint_states->name.size(), 1);
96  ASSERT_EQ(joint_states->name[0], "joint0");
97  ASSERT_EQ(joint_states->velocity.size(), 1);
98  ASSERT_NEAR(joint_states->velocity[0], 1.0, 0.1)
99  << "Valid joint_trajectory must not be ignored";
100 
101  // Stop
102  cmd.header.stamp = clock.clock;
103  cmd.points[0].positions[0] = 0.0;
104  cmd.points[0].velocities[0] = 0.0;
105  pub_cmd.publish(cmd);
106  wait.sleep();
107  for (int i = 0; i < 50; ++i)
108  {
109  clock.clock += clock_step;
110  pub_clock.publish(clock);
111  wait.sleep();
112  ros::spinOnce();
113  }
114  ASSERT_NEAR(joint_states->velocity[0], 0.0, 0.1)
115  << "Valid joint_trajectory must not be ignored";
116 
117  // Publish outdated command
118  cmd.header.stamp = clock.clock - ros::Duration(2.0);
119  cmd.points[0].positions[0] = 1.0;
120  cmd.points[0].velocities[0] = 1.0;
121  pub_cmd.publish(cmd);
122  wait.sleep();
123  for (int i = 0; i < 50; ++i)
124  {
125  clock.clock += clock_step;
126  pub_clock.publish(clock);
127  wait.sleep();
128  ros::spinOnce();
129  }
130  ASSERT_NEAR(joint_states->velocity[0], 0.0, 0.1)
131  << "Outdated joint_trajectory must be ignored";
132 }
133 
134 int main(int argc, char **argv)
135 {
136  testing::InitGoogleTest(&argc, argv);
137  ros::init(argc, argv, "test_joint_trajectory");
138 
139  return RUN_ALL_TESTS();
140 }
TEST(JointTrajectory, CommandValidation)
void publish(const boost::shared_ptr< M > &message) const
int main(int argc, char **argv)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool sleep() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static WallTime now()
ROSCPP_DECL void spinOnce()


ypspur_ros
Author(s): Atsushi Watanabe
autogenerated on Thu Jun 6 2019 19:41:30