moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
planning_scene_monitor_test.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2023, University of Hamburg
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 copyright holder 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: Michael 'v4hn' Goerner
36 Desc: Tests for PlanningSceneMonitor
37*/
38
39// ROS
40#include <rclcpp/rclcpp.hpp>
41
42// Testing
43#include <gtest/gtest.h>
44
45// Main class
48
49class PlanningSceneMonitorTest : public ::testing::Test
50{
51public:
52 void SetUp() override
53 {
54 test_node_ = std::make_shared<rclcpp::Node>("moveit_planning_scene_monitor_test");
55 executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
56 planning_scene_monitor_ = std::make_unique<planning_scene_monitor::PlanningSceneMonitor>(
57 test_node_, "robot_description", "planning_scene_monitor");
58 planning_scene_monitor_->monitorDiffs(true);
59 scene_ = planning_scene_monitor_->getPlanningScene();
60 executor_->add_node(test_node_);
61 executor_thread_ = std::thread([this]() { executor_->spin(); });
62
63 // Needed to avoid race conditions on high-load CPUs.
64 std::this_thread::sleep_for(std::chrono::seconds{ 1 });
65 }
66
67 void TearDown() override
68 {
69 scene_.reset();
70 executor_->cancel();
71 if (executor_thread_.joinable())
72 {
73 executor_thread_.join();
74 }
75 }
76
77protected:
78 std::shared_ptr<rclcpp::Node> test_node_;
79
80 // Executor and a thread to run the executor.
81 rclcpp::Executor::SharedPtr executor_;
82 std::thread executor_thread_;
83
84 planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
85 planning_scene::PlanningScenePtr scene_;
86};
87
88// various code expects the monitored scene to remain the same
89TEST_F(PlanningSceneMonitorTest, TestPersistentScene)
90{
91 auto scene{ planning_scene_monitor_->getPlanningScene() };
92 moveit_msgs::msg::PlanningScene msg;
93 msg.is_diff = msg.robot_state.is_diff = true;
94 planning_scene_monitor_->newPlanningSceneMessage(msg);
95 EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene());
96 msg.is_diff = msg.robot_state.is_diff = false;
97 planning_scene_monitor_->newPlanningSceneMessage(msg);
98 EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene());
99}
100
102
103#define TRIGGERS_UPDATE(msg, expected_update_type) \
104 { \
105 planning_scene_monitor_->clearUpdateCallbacks(); \
106 auto received_update_type{ UpdateType::UPDATE_NONE }; \
107 planning_scene_monitor_->addUpdateCallback([&](auto type) { received_update_type = type; }); \
108 planning_scene_monitor_->newPlanningSceneMessage(msg); \
109 EXPECT_EQ(received_update_type, expected_update_type); \
110 }
111
113{
114 moveit_msgs::msg::PlanningScene msg;
115 msg.is_diff = msg.robot_state.is_diff = true;
116 TRIGGERS_UPDATE(msg, UpdateType::UPDATE_NONE);
117
118 msg.fixed_frame_transforms.emplace_back();
119 msg.fixed_frame_transforms.back().header.frame_id = "base_link";
120 msg.fixed_frame_transforms.back().child_frame_id = "object";
121 msg.fixed_frame_transforms.back().transform.rotation.w = 1.0;
122 TRIGGERS_UPDATE(msg, UpdateType::UPDATE_TRANSFORMS);
123 msg.fixed_frame_transforms.clear();
124 moveit::core::robotStateToRobotStateMsg(scene_->getCurrentState(), msg.robot_state, false);
125 msg.robot_state.is_diff = true;
126 TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE);
127
128 msg.robot_state.is_diff = false;
129 TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE | UpdateType::UPDATE_GEOMETRY);
130
131 msg.robot_state = moveit_msgs::msg::RobotState{};
132 msg.robot_state.is_diff = true;
133 moveit_msgs::msg::CollisionObject collision_object;
134 collision_object.header.frame_id = "base_link";
135 collision_object.id = "object";
136 collision_object.operation = moveit_msgs::msg::CollisionObject::ADD;
137 collision_object.pose.orientation.w = 1.0;
138 collision_object.primitives.emplace_back();
139 collision_object.primitives.back().type = shape_msgs::msg::SolidPrimitive::SPHERE;
140 collision_object.primitives.back().dimensions = { 1.0 };
141 msg.world.collision_objects.emplace_back(collision_object);
142 TRIGGERS_UPDATE(msg, UpdateType::UPDATE_GEOMETRY);
143
144 msg.world.collision_objects.clear();
145 msg.is_diff = false;
146
147 TRIGGERS_UPDATE(msg, UpdateType::UPDATE_SCENE);
148}
149
150int main(int argc, char** argv)
151{
152 rclcpp::init(argc, argv);
153 ::testing::InitGoogleTest(&argc, argv);
154 int result = RUN_ALL_TESTS();
155 rclcpp::shutdown();
156 return result;
157}
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
rclcpp::Executor::SharedPtr executor_
planning_scene::PlanningScenePtr scene_
std::shared_ptr< rclcpp::Node > test_node_
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
int main(int argc, char **argv)
TEST_F(PlanningSceneMonitorTest, TestPersistentScene)
#define TRIGGERS_UPDATE(msg, expected_update_type)