43 #include <gtest/gtest.h>
48 #include <sensor_msgs/JointState.h>
61 geometry_msgs::Pose p;
62 p.orientation.w = 1.0;
66 .
addGroup({
"a" }, {
"a-b-joint" },
"group_a")
71 csm = std::make_unique<planning_scene_monitor::CurrentStateMonitor>(
robot_model_ptr,
nullptr);
72 csm->startStateMonitor();
74 js_a.name = {
"a-b-joint" };
75 js_a.position = { 0.1 };
76 js_a.velocity = { 0.1 };
79 js_b.name = {
"b-c-joint" };
80 js_b.position = { 0.2 };
81 js_b.velocity = { 0.2 };
84 js_ab.name = {
"a-b-joint",
"b-c-joint" };
85 js_ab.position = { 0.3, 0.3 };
86 js_ab.velocity = { 0.3, 0.3 };
102 std::promise<void> promise;
103 auto future = promise.get_future();
104 csm->addUpdateCallback([&promise](
const sensor_msgs::JointStateConstPtr& ) { promise.set_value(); });
107 std::future_status status{ std::future_status::timeout };
108 EXPECT_NO_THROW(status = future.wait_for(std::chrono::seconds{ 1 }));
109 csm->clearUpdateCallbacks();
110 EXPECT_EQ(std::future_status::ready, status);
116 planning_scene_monitor::CurrentStateMonitorPtr
csm;
126 sendJointStateAndWait(js_a);
130 sendJointStateAndWait(js_ab);
137 sendJointStateAndWait(js_a);
138 EXPECT_EQ(js_a.position[0], csm->getCurrentState()->getVariablePosition(
"a-b-joint"));
140 sendJointStateAndWait(js_ab);
141 std::vector<double> positions;
142 csm->getCurrentState()->copyJointGroupPositions(
"group", positions);
148 sendJointStateAndWait(js_a);
151 sendJointStateAndWait(js_b);
152 EXPECT_EQ(js_b.header.stamp, csm->getCurrentStateTime())
153 <<
"older partial joint state was ignored in current state retrieval!";
155 js_b.position = { 0.25 };
157 sendJointStateAndWait(js_b);
158 EXPECT_EQ(js_a.header.stamp, csm->getCurrentStateTime())
159 <<
"older partial joint state was ignored in current state retrieval!";
161 sendJointStateAndWait(js_ab);
162 EXPECT_EQ(js_ab.header.stamp, csm->getCurrentStateTime()) <<
"newer stamp did not update csm";
165 sendJointStateAndWait(js_a_old);
167 <<
"jumped back in time for a known joint, but csm still claims it knows all joints";
168 EXPECT_EQ(csm->getCurrentStateTime(),
ros::Time()) <<
"jumping back for a known joint did not reset state time";
169 EXPECT_EQ(js_a_old.position[0], csm->getCurrentState()->getVariablePosition(
"a-b-joint"));
174 sendJointStateAndWait(js_a);
175 EXPECT_EQ(csm->getCurrentStateTime(),
ros::Time()) <<
"incomplete state should have 0 time";
177 sendJointStateAndWait(js_b);
178 EXPECT_EQ(js_b.header.stamp, csm->getCurrentStateTime())
179 <<
"older partial joint state was ignored in current state retrieval!";
180 EXPECT_EQ(js_a.header.stamp, csm->getCurrentStateTime(
"group_a"))
181 <<
"Group is aware of the timestamp of non-group joints!";
183 js_b.position = { 0.25 };
185 sendJointStateAndWait(js_b);
186 EXPECT_EQ(js_a.header.stamp, csm->getCurrentStateTime())
187 <<
"older partial joint state was ignored in current state retrieval!";
190 int main(
int argc,
char** argv)
192 testing::InitGoogleTest(&argc, argv);
193 ros::init(argc, argv,
"current_state_monitor_test");
198 int result = RUN_ALL_TESTS();