Merge branch 'master' into ros2

This commit is contained in:
Robert Haschke 2025-10-09 15:33:51 +02:00
commit 35063ff0dd
6 changed files with 42 additions and 26 deletions

View File

@ -240,7 +240,8 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::msg::Solution& msg, I
if (trajectory())
trajectory()->getRobotTrajectoryMsg(t.trajectory);
if (this->end()->scene()->getParent() == this->start()->scene())
if (this->end()->scene()->getParent() == this->start()->scene() || // diff
this->end()->scene() == this->start()->scene()) // identical (from generator)
this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
else
this->end()->scene()->getPlanningSceneMsg(t.scene_diff);

View File

@ -50,6 +50,7 @@ if (BUILD_TESTING)
mtc_add_gmock(test_pruning.cpp)
mtc_add_gtest(test_properties.cpp)
mtc_add_gtest(test_cost_terms.cpp)
mtc_add_gtest(test_storage.cpp)
mtc_add_gmock(test_fallback.cpp)
mtc_add_gmock(test_cost_queue.cpp)

View File

@ -0,0 +1,34 @@
#include "models.h"
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/planning_scene/planning_scene.hpp>
#include <gtest/gtest.h>
using namespace moveit::task_constructor;
using namespace planning_scene;
using namespace moveit::core;
// https://github.com/moveit/moveit_task_constructor/issues/638
TEST(SolutionMsg, DuplicateScenes) {
Task t;
PlanningScenePtr scene;
t.setRobotModel(getModel());
scene = std::make_shared<PlanningScene>(t.getRobotModel());
t.add(std::make_unique<stages::FixedState>("start", scene));
EXPECT_TRUE(t.plan(1));
EXPECT_EQ(t.solutions().size(), 1u);
// create solution
moveit_task_constructor_msgs::msg::Solution solution_msg;
t.solutions().front()->toMsg(solution_msg);
// all sub trajectories `scene_diff` should be a diff
EXPECT_EQ(solution_msg.sub_trajectory.size(), 1u);
EXPECT_EQ(solution_msg.start_scene.is_diff, false);
EXPECT_EQ(solution_msg.sub_trajectory.front().scene_diff.is_diff, true);
}

View File

@ -2,16 +2,13 @@ set(MOVEIT_LIB_NAME motion_planning_tasks_properties)
set(SOURCES
property_factory.cpp
property_from_yaml.cpp
)
find_package(PkgConfig REQUIRED)
pkg_check_modules(YAML yaml-0.1)
if (YAML_FOUND)
# Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually
find_library(YAML_LIBRARIES ${YAML_LIBRARIES} PATHS ${YAML_LIBRARY_DIRS})
list(APPEND SOURCES property_from_yaml.cpp)
add_definitions(-DHAVE_YAML)
endif()
pkg_check_modules(YAML REQUIRED yaml-0.1)
# Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually
find_library(YAML_LIBRARIES ${YAML_LIBRARIES} PATHS ${YAML_LIBRARY_DIRS})
add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES})

View File

@ -148,22 +148,4 @@ void PropertyFactory::addRemainingProperties(rviz_common::properties::Property*
new rviz_common::properties::Property("no properties", QVariant(), QString(), root);
}
#ifndef HAVE_YAML
rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/,
const std::string& description,
const std::string& value,
rviz_common::properties::Property* old) {
if (old) { // reuse existing Property?
assert(old->getNameStd() == name);
old->setDescription(QString::fromStdString(description));
old->setValue(QString::fromStdString(value));
return old;
} else { // create new Property?
rviz_common::properties::Property* result = new rviz::StringProperty(
QString::fromStdString(name), QString::fromStdString(value), QString::fromStdString(description));
result->setReadOnly(true);
return result;
}
}
#endif
} // namespace moveit_rviz_plugin

View File

@ -18,6 +18,7 @@
<depend>moveit_ros_visualization</depend>
<depend>rclcpp</depend>
<depend>rviz2</depend>
<depend>yaml</depend>
<test_depend>ament_cmake_gmock</test_depend>
<test_depend>ament_cmake_gtest</test_depend>