Skip to content

Commit 89add74

Browse files
sjahrv4hn
authored and
rhaschke
committed
PSM: keep references to scene_ valid upon receiving full scenes (moveit#2745)
plan_execution-related modules rely on `plan.planning_scene_` in many places to point to the currently monitored scene (or a diff on top of it). Before this patch, if the PSM would receive full scenes during execution, `plan.planning_scene_` would not include later incremental updates anymore because the monitor created a new diff scene. --------- Co-authored-by: v4hn <me@v4hn.de>
1 parent 7ffdfd3 commit 89add74

File tree

5 files changed

+270
-18
lines changed

5 files changed

+270
-18
lines changed

‎moveit_ros/planning/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,7 @@
4545
<test_depend>ament_lint_common</test_depend>
4646
<test_depend>ament_cmake_gmock</test_depend>
4747
<test_depend>ament_cmake_gtest</test_depend>
48+
<test_depend>moveit_configs_utils</test_depend>
4849
<test_depend>ros_testing</test_depend>
4950

5051
<test_depend>moveit_resources_panda_moveit_config</test_depend>

‎moveit_ros/planning/planning_scene_monitor/CMakeLists.txt

+8
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${MOVEIT_LIB_NAME}_export.h DESTINATIO
4646
if(BUILD_TESTING)
4747
find_package(ament_cmake_gtest REQUIRED)
4848
find_package(ament_cmake_gmock REQUIRED)
49+
find_package(ros_testing REQUIRED)
4950

5051
ament_add_gmock(current_state_monitor_tests
5152
test/current_state_monitor_tests.cpp
@@ -59,4 +60,11 @@ if(BUILD_TESTING)
5960
target_link_libraries(trajectory_monitor_tests
6061
${MOVEIT_LIB_NAME}
6162
)
63+
64+
ament_add_gtest_executable(planning_scene_monitor_test
65+
test/planning_scene_monitor_test.cpp)
66+
target_link_libraries(planning_scene_monitor_test ${MOVEIT_LIB_NAME})
67+
ament_target_dependencies(planning_scene_monitor_test moveit_core rclcpp moveit_msgs)
68+
add_ros_test(test/launch/planning_scene_monitor.test.py TIMEOUT 30 ARGS
69+
"test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
6270
endif()

‎moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

+15-18
Original file line numberDiff line numberDiff line change
@@ -666,7 +666,21 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann
666666
RCLCPP_DEBUG(LOGGER, "scene update %f robot stamp: %f", fmod(last_update_time_.seconds(), 10.),
667667
fmod(last_robot_motion_time_.seconds(), 10.));
668668
old_scene_name = scene_->getName();
669-
result = scene_->usePlanningSceneMsg(scene);
669+
670+
if (!scene.is_diff && parent_scene_)
671+
{
672+
// clear maintained (diff) scene_ and set the full new scene in parent_scene_ instead
673+
scene_->clearDiffs();
674+
result = parent_scene_->setPlanningSceneMsg(scene);
675+
// There were no callbacks for individual object changes, so rebuild the octree masks
676+
excludeAttachedBodiesFromOctree();
677+
excludeWorldObjectsFromOctree();
678+
}
679+
else
680+
{
681+
result = scene_->setPlanningSceneDiffMsg(scene);
682+
}
683+
670684
if (octomap_monitor_)
671685
{
672686
if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
@@ -678,23 +692,6 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann
678692
}
679693
robot_model_ = scene_->getRobotModel();
680694

681-
// if we just reset the scene completely but we were maintaining diffs, we need to fix that
682-
if (!scene.is_diff && parent_scene_)
683-
{
684-
// the scene is now decoupled from the parent, since we just reset it
685-
scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
686-
scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
687-
parent_scene_ = scene_;
688-
scene_ = parent_scene_->diff();
689-
scene_const_ = scene_;
690-
scene_->setAttachedBodyUpdateCallback([this](moveit::core::AttachedBody* body, bool attached) {
691-
currentStateAttachedBodyUpdateCallback(body, attached);
692-
});
693-
scene_->setCollisionObjectUpdateCallback(
694-
[this](const collision_detection::World::ObjectConstPtr& object, collision_detection::World::Action action) {
695-
currentWorldObjectUpdateCallback(object, action);
696-
});
697-
}
698695
if (octomap_monitor_)
699696
{
700697
excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in

‎moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py

+92
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
import os
2+
import launch
3+
import unittest
4+
import launch_ros
5+
import launch_testing
6+
from ament_index_python.packages import get_package_share_directory
7+
from moveit_configs_utils import MoveItConfigsBuilder
8+
9+
10+
def generate_test_description():
11+
moveit_config = (
12+
MoveItConfigsBuilder("moveit_resources_panda")
13+
.robot_description(file_path="config/panda.urdf.xacro")
14+
.to_moveit_configs()
15+
)
16+
17+
# ros2_control using FakeSystem as hardware
18+
ros2_controllers_path = os.path.join(
19+
get_package_share_directory("moveit_resources_panda_moveit_config"),
20+
"config",
21+
"ros2_controllers.yaml",
22+
)
23+
ros2_control_node = launch_ros.actions.Node(
24+
package="controller_manager",
25+
executable="ros2_control_node",
26+
parameters=[ros2_controllers_path],
27+
remappings=[
28+
("/controller_manager/robot_description", "/robot_description"),
29+
],
30+
output="screen",
31+
)
32+
33+
joint_state_broadcaster_spawner = launch_ros.actions.Node(
34+
package="controller_manager",
35+
executable="spawner",
36+
arguments=[
37+
"joint_state_broadcaster",
38+
"--controller-manager-timeout",
39+
"300",
40+
"--controller-manager",
41+
"/controller_manager",
42+
],
43+
output="screen",
44+
)
45+
46+
panda_arm_controller_spawner = launch_ros.actions.Node(
47+
package="controller_manager",
48+
executable="spawner",
49+
arguments=["panda_arm_controller", "-c", "/controller_manager"],
50+
)
51+
52+
psm_gtest = launch_ros.actions.Node(
53+
executable=launch.substitutions.PathJoinSubstitution(
54+
[
55+
launch.substitutions.LaunchConfiguration("test_binary_dir"),
56+
"planning_scene_monitor_test",
57+
]
58+
),
59+
parameters=[
60+
moveit_config.to_dict(),
61+
],
62+
output="screen",
63+
)
64+
65+
return launch.LaunchDescription(
66+
[
67+
launch.actions.TimerAction(period=2.0, actions=[ros2_control_node]),
68+
launch.actions.TimerAction(
69+
period=4.0, actions=[joint_state_broadcaster_spawner]
70+
),
71+
launch.actions.TimerAction(
72+
period=6.0, actions=[panda_arm_controller_spawner]
73+
),
74+
launch.actions.TimerAction(period=9.0, actions=[psm_gtest]),
75+
launch_testing.actions.ReadyToTest(),
76+
]
77+
), {
78+
"psm_gtest": psm_gtest,
79+
}
80+
81+
82+
class TestGTestWaitForCompletion(unittest.TestCase):
83+
# Waits for test to complete, then waits a bit to make sure result files are generated
84+
def test_gtest_run_complete(self, psm_gtest):
85+
self.proc_info.assertWaitForShutdown(psm_gtest, timeout=4000.0)
86+
87+
88+
@launch_testing.post_shutdown_test()
89+
class TestGTestProcessPostShutdown(unittest.TestCase):
90+
# Checks if the test has been completed with acceptable exit codes (successful codes)
91+
def test_gtest_pass(self, proc_info, psm_gtest):
92+
launch_testing.asserts.assertExitCodes(proc_info, process=psm_gtest)

‎moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp

+154
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
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
46+
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
47+
#include <moveit/robot_state/conversions.h>
48+
49+
class PlanningSceneMonitorTest : public ::testing::Test
50+
{
51+
public:
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+
64+
void TearDown() override
65+
{
66+
scene_.reset();
67+
executor_->cancel();
68+
if (executor_thread_.joinable())
69+
{
70+
executor_thread_.join();
71+
}
72+
}
73+
74+
protected:
75+
std::shared_ptr<rclcpp::Node> test_node_;
76+
77+
// Executor and a thread to run the executor.
78+
rclcpp::Executor::SharedPtr executor_;
79+
std::thread executor_thread_;
80+
81+
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
82+
planning_scene::PlanningScenePtr scene_;
83+
};
84+
85+
// various code expects the monitored scene to remain the same
86+
TEST_F(PlanningSceneMonitorTest, TestPersistentScene)
87+
{
88+
auto scene{ planning_scene_monitor_->getPlanningScene() };
89+
moveit_msgs::msg::PlanningScene msg;
90+
msg.is_diff = msg.robot_state.is_diff = true;
91+
planning_scene_monitor_->newPlanningSceneMessage(msg);
92+
EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene());
93+
msg.is_diff = msg.robot_state.is_diff = false;
94+
planning_scene_monitor_->newPlanningSceneMessage(msg);
95+
EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene());
96+
}
97+
98+
using UpdateType = planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType;
99+
100+
#define TRIGGERS_UPDATE(msg, expected_update_type) \
101+
{ \
102+
planning_scene_monitor_->clearUpdateCallbacks(); \
103+
auto received_update_type{ UpdateType::UPDATE_NONE }; \
104+
planning_scene_monitor_->addUpdateCallback([&](auto type) { received_update_type = type; }); \
105+
planning_scene_monitor_->newPlanningSceneMessage(msg); \
106+
EXPECT_EQ(received_update_type, expected_update_type); \
107+
}
108+
109+
TEST_F(PlanningSceneMonitorTest, UpdateTypes)
110+
{
111+
moveit_msgs::msg::PlanningScene msg;
112+
msg.is_diff = msg.robot_state.is_diff = true;
113+
TRIGGERS_UPDATE(msg, UpdateType::UPDATE_NONE);
114+
115+
msg.fixed_frame_transforms.emplace_back();
116+
msg.fixed_frame_transforms.back().header.frame_id = "base_link";
117+
msg.fixed_frame_transforms.back().child_frame_id = "object";
118+
msg.fixed_frame_transforms.back().transform.rotation.w = 1.0;
119+
TRIGGERS_UPDATE(msg, UpdateType::UPDATE_TRANSFORMS);
120+
msg.fixed_frame_transforms.clear();
121+
moveit::core::robotStateToRobotStateMsg(scene_->getCurrentState(), msg.robot_state, false);
122+
msg.robot_state.is_diff = true;
123+
TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE);
124+
125+
msg.robot_state.is_diff = false;
126+
TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE | UpdateType::UPDATE_GEOMETRY);
127+
128+
msg.robot_state = moveit_msgs::msg::RobotState{};
129+
msg.robot_state.is_diff = true;
130+
moveit_msgs::msg::CollisionObject collision_object;
131+
collision_object.header.frame_id = "base_link";
132+
collision_object.id = "object";
133+
collision_object.operation = moveit_msgs::msg::CollisionObject::ADD;
134+
collision_object.pose.orientation.w = 1.0;
135+
collision_object.primitives.emplace_back();
136+
collision_object.primitives.back().type = shape_msgs::msg::SolidPrimitive::SPHERE;
137+
collision_object.primitives.back().dimensions = { 1.0 };
138+
msg.world.collision_objects.emplace_back(collision_object);
139+
TRIGGERS_UPDATE(msg, UpdateType::UPDATE_GEOMETRY);
140+
141+
msg.world.collision_objects.clear();
142+
msg.is_diff = false;
143+
144+
TRIGGERS_UPDATE(msg, UpdateType::UPDATE_SCENE);
145+
}
146+
147+
int main(int argc, char** argv)
148+
{
149+
rclcpp::init(argc, argv);
150+
::testing::InitGoogleTest(&argc, argv);
151+
int result = RUN_ALL_TESTS();
152+
rclcpp::shutdown();
153+
return result;
154+
}

0 commit comments

Comments
 (0)