Skip to content

Commit

Permalink
PSM: keep references to scene_ valid upon receiving full scenes (move…
Browse files Browse the repository at this point in the history
…it#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 <[email protected]>
  • Loading branch information
2 people authored and rhaschke committed May 24, 2024
1 parent 7ffdfd3 commit 89add74
Show file tree
Hide file tree
Showing 5 changed files with 270 additions and 18 deletions.
1 change: 1 addition & 0 deletions moveit_ros/planning/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_cmake_gmock</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>moveit_configs_utils</test_depend>
<test_depend>ros_testing</test_depend>

<test_depend>moveit_resources_panda_moveit_config</test_depend>
Expand Down
8 changes: 8 additions & 0 deletions moveit_ros/planning/planning_scene_monitor/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@ install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${MOVEIT_LIB_NAME}_export.h DESTINATIO
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_cmake_gmock REQUIRED)
find_package(ros_testing REQUIRED)

ament_add_gmock(current_state_monitor_tests
test/current_state_monitor_tests.cpp
Expand All @@ -59,4 +60,11 @@ if(BUILD_TESTING)
target_link_libraries(trajectory_monitor_tests
${MOVEIT_LIB_NAME}
)

ament_add_gtest_executable(planning_scene_monitor_test
test/planning_scene_monitor_test.cpp)
target_link_libraries(planning_scene_monitor_test ${MOVEIT_LIB_NAME})
ament_target_dependencies(planning_scene_monitor_test moveit_core rclcpp moveit_msgs)
add_ros_test(test/launch/planning_scene_monitor.test.py TIMEOUT 30 ARGS
"test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
endif()
Original file line number Diff line number Diff line change
Expand Up @@ -666,7 +666,21 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann
RCLCPP_DEBUG(LOGGER, "scene update %f robot stamp: %f", fmod(last_update_time_.seconds(), 10.),
fmod(last_robot_motion_time_.seconds(), 10.));
old_scene_name = scene_->getName();
result = scene_->usePlanningSceneMsg(scene);

if (!scene.is_diff && parent_scene_)
{
// clear maintained (diff) scene_ and set the full new scene in parent_scene_ instead
scene_->clearDiffs();
result = parent_scene_->setPlanningSceneMsg(scene);
// There were no callbacks for individual object changes, so rebuild the octree masks
excludeAttachedBodiesFromOctree();
excludeWorldObjectsFromOctree();
}
else
{
result = scene_->setPlanningSceneDiffMsg(scene);
}

if (octomap_monitor_)
{
if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
Expand All @@ -678,23 +692,6 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann
}
robot_model_ = scene_->getRobotModel();

// if we just reset the scene completely but we were maintaining diffs, we need to fix that
if (!scene.is_diff && parent_scene_)
{
// the scene is now decoupled from the parent, since we just reset it
scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback());
scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
parent_scene_ = scene_;
scene_ = parent_scene_->diff();
scene_const_ = scene_;
scene_->setAttachedBodyUpdateCallback([this](moveit::core::AttachedBody* body, bool attached) {
currentStateAttachedBodyUpdateCallback(body, attached);
});
scene_->setCollisionObjectUpdateCallback(
[this](const collision_detection::World::ObjectConstPtr& object, collision_detection::World::Action action) {
currentWorldObjectUpdateCallback(object, action);
});
}
if (octomap_monitor_)
{
excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
import os
import launch
import unittest
import launch_ros
import launch_testing
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder


def generate_test_description():
moveit_config = (
MoveItConfigsBuilder("moveit_resources_panda")
.robot_description(file_path="config/panda.urdf.xacro")
.to_moveit_configs()
)

# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"ros2_controllers.yaml",
)
ros2_control_node = launch_ros.actions.Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[ros2_controllers_path],
remappings=[
("/controller_manager/robot_description", "/robot_description"),
],
output="screen",
)

joint_state_broadcaster_spawner = launch_ros.actions.Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster",
"--controller-manager-timeout",
"300",
"--controller-manager",
"/controller_manager",
],
output="screen",
)

panda_arm_controller_spawner = launch_ros.actions.Node(
package="controller_manager",
executable="spawner",
arguments=["panda_arm_controller", "-c", "/controller_manager"],
)

psm_gtest = launch_ros.actions.Node(
executable=launch.substitutions.PathJoinSubstitution(
[
launch.substitutions.LaunchConfiguration("test_binary_dir"),
"planning_scene_monitor_test",
]
),
parameters=[
moveit_config.to_dict(),
],
output="screen",
)

return launch.LaunchDescription(
[
launch.actions.TimerAction(period=2.0, actions=[ros2_control_node]),
launch.actions.TimerAction(
period=4.0, actions=[joint_state_broadcaster_spawner]
),
launch.actions.TimerAction(
period=6.0, actions=[panda_arm_controller_spawner]
),
launch.actions.TimerAction(period=9.0, actions=[psm_gtest]),
launch_testing.actions.ReadyToTest(),
]
), {
"psm_gtest": psm_gtest,
}


class TestGTestWaitForCompletion(unittest.TestCase):
# Waits for test to complete, then waits a bit to make sure result files are generated
def test_gtest_run_complete(self, psm_gtest):
self.proc_info.assertWaitForShutdown(psm_gtest, timeout=4000.0)


@launch_testing.post_shutdown_test()
class TestGTestProcessPostShutdown(unittest.TestCase):
# Checks if the test has been completed with acceptable exit codes (successful codes)
def test_gtest_pass(self, proc_info, psm_gtest):
launch_testing.asserts.assertExitCodes(proc_info, process=psm_gtest)
Original file line number Diff line number Diff line change
@@ -0,0 +1,154 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2023, University of Hamburg
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Michael 'v4hn' Goerner
Desc: Tests for PlanningSceneMonitor
*/

// ROS
#include <rclcpp/rclcpp.hpp>

// Testing
#include <gtest/gtest.h>

// Main class
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/robot_state/conversions.h>

class PlanningSceneMonitorTest : public ::testing::Test
{
public:
void SetUp() override
{
test_node_ = std::make_shared<rclcpp::Node>("moveit_planning_scene_monitor_test");
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
planning_scene_monitor_ = std::make_unique<planning_scene_monitor::PlanningSceneMonitor>(
test_node_, "robot_description", "planning_scene_monitor");
planning_scene_monitor_->monitorDiffs(true);
scene_ = planning_scene_monitor_->getPlanningScene();
executor_->add_node(test_node_);
executor_thread_ = std::thread([this]() { executor_->spin(); });
}

void TearDown() override
{
scene_.reset();
executor_->cancel();
if (executor_thread_.joinable())
{
executor_thread_.join();
}
}

protected:
std::shared_ptr<rclcpp::Node> test_node_;

// Executor and a thread to run the executor.
rclcpp::Executor::SharedPtr executor_;
std::thread executor_thread_;

planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
planning_scene::PlanningScenePtr scene_;
};

// various code expects the monitored scene to remain the same
TEST_F(PlanningSceneMonitorTest, TestPersistentScene)
{
auto scene{ planning_scene_monitor_->getPlanningScene() };
moveit_msgs::msg::PlanningScene msg;
msg.is_diff = msg.robot_state.is_diff = true;
planning_scene_monitor_->newPlanningSceneMessage(msg);
EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene());
msg.is_diff = msg.robot_state.is_diff = false;
planning_scene_monitor_->newPlanningSceneMessage(msg);
EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene());
}

using UpdateType = planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType;

#define TRIGGERS_UPDATE(msg, expected_update_type) \
{ \
planning_scene_monitor_->clearUpdateCallbacks(); \
auto received_update_type{ UpdateType::UPDATE_NONE }; \
planning_scene_monitor_->addUpdateCallback([&](auto type) { received_update_type = type; }); \
planning_scene_monitor_->newPlanningSceneMessage(msg); \
EXPECT_EQ(received_update_type, expected_update_type); \
}

TEST_F(PlanningSceneMonitorTest, UpdateTypes)
{
moveit_msgs::msg::PlanningScene msg;
msg.is_diff = msg.robot_state.is_diff = true;
TRIGGERS_UPDATE(msg, UpdateType::UPDATE_NONE);

msg.fixed_frame_transforms.emplace_back();
msg.fixed_frame_transforms.back().header.frame_id = "base_link";
msg.fixed_frame_transforms.back().child_frame_id = "object";
msg.fixed_frame_transforms.back().transform.rotation.w = 1.0;
TRIGGERS_UPDATE(msg, UpdateType::UPDATE_TRANSFORMS);
msg.fixed_frame_transforms.clear();
moveit::core::robotStateToRobotStateMsg(scene_->getCurrentState(), msg.robot_state, false);
msg.robot_state.is_diff = true;
TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE);

msg.robot_state.is_diff = false;
TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE | UpdateType::UPDATE_GEOMETRY);

msg.robot_state = moveit_msgs::msg::RobotState{};
msg.robot_state.is_diff = true;
moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = "base_link";
collision_object.id = "object";
collision_object.operation = moveit_msgs::msg::CollisionObject::ADD;
collision_object.pose.orientation.w = 1.0;
collision_object.primitives.emplace_back();
collision_object.primitives.back().type = shape_msgs::msg::SolidPrimitive::SPHERE;
collision_object.primitives.back().dimensions = { 1.0 };
msg.world.collision_objects.emplace_back(collision_object);
TRIGGERS_UPDATE(msg, UpdateType::UPDATE_GEOMETRY);

msg.world.collision_objects.clear();
msg.is_diff = false;

TRIGGERS_UPDATE(msg, UpdateType::UPDATE_SCENE);
}

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
::testing::InitGoogleTest(&argc, argv);
int result = RUN_ALL_TESTS();
rclcpp::shutdown();
return result;
}

0 comments on commit 89add74

Please sign in to comment.