diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 3a54ba8d..8e4de1c8 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -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); diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 095c0a59..dadfb18d 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -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) diff --git a/core/test/test_storage.cpp b/core/test/test_storage.cpp new file mode 100644 index 00000000..d7e082d1 --- /dev/null +++ b/core/test/test_storage.cpp @@ -0,0 +1,34 @@ +#include "models.h" + +#include +#include + +#include + +#include + +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(t.getRobotModel()); + t.add(std::make_unique("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); +} diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt index a0796a34..d839e91b 100644 --- a/visualization/motion_planning_tasks/properties/CMakeLists.txt +++ b/visualization/motion_planning_tasks/properties/CMakeLists.txt @@ -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}) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 3c3af2a1..3df7fa4c 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -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 diff --git a/visualization/package.xml b/visualization/package.xml index 9fca78b1..f5cbcf17 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -18,6 +18,7 @@ moveit_ros_visualization rclcpp rviz2 + yaml ament_cmake_gmock ament_cmake_gtest