Skip to content

Commit d9aa803

Browse files
mosfet80mosfet80
authored
Merge branch 'moveit:main' into patch-3
2 parents e5c9a4e + 3268f16 commit d9aa803

File tree

42 files changed

+7357
-1669
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

42 files changed

+7357
-1669
lines changed

‎.codespell_words

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
aas
22
ang
33
ans
4+
AppendT
45
atleast
56
ba
67
brin

‎.docker/tutorial-source/Dockerfile

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44
# Source build of the repos file from the tutorial site
55

66
ARG ROS_DISTRO=rolling
7-
ARG GZ_VERSION=harmonic
7+
ARG GZ_VERSION=ionic
88

99
FROM moveit/moveit2:${ROS_DISTRO}-ci
1010
LABEL maintainer Tyler Weaver [email protected]

‎.github/workflows/ci.yaml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ jobs:
3232
# TODO(andyz): When this clang-tidy issue is fixed, remove -Wno-unknown-warning-option
3333
# https://stackoverflow.com/a/41673702
3434
CXXFLAGS: >-
35-
-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option -Wno-cpp
35+
-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option
3636
CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
3737
DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }}
3838
UPSTREAM_WORKSPACE: >

‎README.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ The [MoveIt Motion Planning Framework for ROS 2](http://moveit.ros.org). For the
88

99
[![Formatting (pre-commit)](https://github.com/moveit/moveit2/actions/workflows/format.yaml/badge.svg?branch=main)](https://github.com/moveit/moveit2/actions/workflows/format.yaml?query=branch%3Amain)
1010
[![CI (Rolling and Humble)](https://github.com/moveit/moveit2/actions/workflows/ci.yaml/badge.svg?branch=main)](https://github.com/moveit/moveit2/actions/workflows/ci.yaml?query=branch%3Amain)
11-
[![Code Coverage](https://codecov.io/gh/ros-planning/moveit2/branch/main/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit2)
11+
[![Code Coverage](https://codecov.io/gh/moveit/moveit2/branch/main/graph/badge.svg?token=QC1O0VzGpm)](https://codecov.io/gh/moveit/moveit2)
1212

1313
## Getting Started
1414

‎moveit_core/planning_scene/src/planning_scene.cpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -374,6 +374,7 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene)
374374
scene->world_->removeObject(it.first);
375375
scene->removeObjectColor(it.first);
376376
scene->removeObjectType(it.first);
377+
scene->getAllowedCollisionMatrixNonConst().removeEntry(it.first);
377378
}
378379
else
379380
{
@@ -1465,6 +1466,7 @@ void PlanningScene::removeAllCollisionObjects()
14651466
world_->removeObject(object_id);
14661467
removeObjectColor(object_id);
14671468
removeObjectType(object_id);
1469+
getAllowedCollisionMatrixNonConst().removeEntry(object_id);
14681470
}
14691471
}
14701472
}
@@ -1939,6 +1941,7 @@ bool PlanningScene::processCollisionObjectRemove(const moveit_msgs::msg::Collisi
19391941

19401942
removeObjectColor(object.id);
19411943
removeObjectType(object.id);
1944+
getAllowedCollisionMatrixNonConst().removeEntry(object.id);
19421945
}
19431946
return true;
19441947
}

‎moveit_core/planning_scene/test/test_planning_scene.cpp

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -590,6 +590,61 @@ TEST(PlanningScene, RobotStateDiffBug)
590590
}
591591
}
592592

593+
TEST(PlanningScene, UpdateACMAfterObjectRemoval)
594+
{
595+
auto robot_model = moveit::core::loadTestingRobotModel("panda");
596+
auto ps = std::make_shared<planning_scene::PlanningScene>(robot_model);
597+
598+
const auto object_name = "object";
599+
collision_detection::CollisionRequest collision_request;
600+
collision_request.group_name = "hand";
601+
collision_request.verbose = true;
602+
603+
// Helper function to add an object to the planning scene
604+
auto add_object = [&] {
605+
const auto ps1 = createPlanningSceneDiff(*ps, object_name, moveit_msgs::msg::CollisionObject::ADD);
606+
ps->usePlanningSceneMsg(ps1);
607+
EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set<std::string>{ object_name }));
608+
};
609+
610+
// Modify the allowed collision matrix and make sure it is updated
611+
auto modify_acm = [&] {
612+
collision_detection::AllowedCollisionMatrix& acm = ps->getAllowedCollisionMatrixNonConst();
613+
acm.setEntry(object_name, ps->getRobotModel()->getJointModelGroup("hand")->getLinkModelNamesWithCollisionGeometry(),
614+
true);
615+
EXPECT_TRUE(ps->getAllowedCollisionMatrix().hasEntry(object_name));
616+
};
617+
618+
// Check collision
619+
auto check_collision = [&] {
620+
collision_detection::CollisionResult res;
621+
ps->checkCollision(collision_request, res);
622+
return res.collision;
623+
};
624+
625+
// Test removing a collision object using a diff
626+
add_object();
627+
EXPECT_TRUE(check_collision());
628+
modify_acm();
629+
EXPECT_FALSE(check_collision());
630+
{
631+
const auto ps1 = createPlanningSceneDiff(*ps, object_name, moveit_msgs::msg::CollisionObject::REMOVE);
632+
ps->usePlanningSceneMsg(ps1);
633+
EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set<std::string>{}));
634+
EXPECT_FALSE(ps->getAllowedCollisionMatrix().hasEntry(object_name));
635+
}
636+
637+
// Test removing all objects
638+
add_object();
639+
// This should report a collision since it's a completely new object
640+
EXPECT_TRUE(check_collision());
641+
modify_acm();
642+
EXPECT_FALSE(check_collision());
643+
ps->removeAllCollisionObjects();
644+
EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set<std::string>{}));
645+
EXPECT_FALSE(ps->getAllowedCollisionMatrix().hasEntry(object_name));
646+
}
647+
593648
#ifndef INSTANTIATE_TEST_SUITE_P // prior to gtest 1.10
594649
#define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__)
595650
#endif

‎moveit_core/robot_model/include/moveit/robot_model/joint_model_group.hpp

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -588,8 +588,10 @@ class JointModelGroup
588588

589589
/**
590590
* @brief Get the lower and upper position limits of all active variables in the group.
591-
*
592-
* @return std::pair<Eigen::VectorXd, Eigen::VectorXd> Containing the lower and upper joint limits for all active variables.
591+
* @details In the case of variable without position bounds (e.g. continuous joints), the lower and upper limits are
592+
* set to infinity.
593+
* @return std::pair<Eigen::VectorXd, Eigen::VectorXd> Containing the lower and upper joint limits for all active
594+
* variables.
593595
*/
594596
[[nodiscard]] std::pair<Eigen::VectorXd, Eigen::VectorXd> getLowerAndUpperLimits() const;
595597

‎moveit_core/robot_model/src/joint_model_group.cpp

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -842,8 +842,10 @@ std::pair<Eigen::VectorXd, Eigen::VectorXd> JointModelGroup::getLowerAndUpperLim
842842
{
843843
for (const moveit::core::VariableBounds& variable_bounds : *joint_bounds)
844844
{
845-
lower_limits[variable_index] = variable_bounds.min_position_;
846-
upper_limits[variable_index] = variable_bounds.max_position_;
845+
lower_limits[variable_index] =
846+
variable_bounds.position_bounded_ ? variable_bounds.min_position_ : -std::numeric_limits<double>::infinity();
847+
upper_limits[variable_index] =
848+
variable_bounds.position_bounded_ ? variable_bounds.max_position_ : std::numeric_limits<double>::infinity();
847849
variable_index++;
848850
}
849851
}

‎moveit_ros/trajectory_cache/CMakeLists.txt

Lines changed: 60 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -28,9 +28,62 @@ set(TRAJECTORY_CACHE_DEPENDENCIES
2828
trajectory_msgs
2929
warehouse_ros)
3030

31+
set(TRAJECTORY_CACHE_LIBRARIES
32+
moveit_ros_trajectory_cache_utils_lib
33+
moveit_ros_trajectory_cache_features_lib
34+
moveit_ros_trajectory_cache_cache_insert_policies_lib
35+
moveit_ros_trajectory_cache_lib)
36+
37+
# Utils library
38+
add_library(moveit_ros_trajectory_cache_utils_lib SHARED src/utils/utils.cpp)
39+
generate_export_header(moveit_ros_trajectory_cache_utils_lib)
40+
target_include_directories(
41+
moveit_ros_trajectory_cache_utils_lib
42+
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
43+
$<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
44+
ament_target_dependencies(moveit_ros_trajectory_cache_utils_lib
45+
${TRAJECTORY_CACHE_DEPENDENCIES})
46+
47+
# Features library
48+
add_library(
49+
moveit_ros_trajectory_cache_features_lib SHARED
50+
src/features/motion_plan_request_features.cpp
51+
src/features/get_cartesian_path_request_features.cpp)
52+
generate_export_header(moveit_ros_trajectory_cache_features_lib)
53+
target_link_libraries(moveit_ros_trajectory_cache_features_lib
54+
moveit_ros_trajectory_cache_utils_lib)
55+
target_include_directories(
56+
moveit_ros_trajectory_cache_features_lib
57+
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
58+
$<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
59+
ament_target_dependencies(moveit_ros_trajectory_cache_features_lib
60+
${TRAJECTORY_CACHE_DEPENDENCIES})
61+
62+
# Cache insert policies library
63+
add_library(
64+
moveit_ros_trajectory_cache_cache_insert_policies_lib SHARED
65+
src/cache_insert_policies/always_insert_never_prune_policy.cpp
66+
src/cache_insert_policies/best_seen_execution_time_policy.cpp)
67+
generate_export_header(moveit_ros_trajectory_cache_cache_insert_policies_lib)
68+
target_link_libraries(
69+
moveit_ros_trajectory_cache_cache_insert_policies_lib
70+
moveit_ros_trajectory_cache_features_lib
71+
moveit_ros_trajectory_cache_utils_lib)
72+
target_include_directories(
73+
moveit_ros_trajectory_cache_cache_insert_policies_lib
74+
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
75+
$<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
76+
ament_target_dependencies(moveit_ros_trajectory_cache_cache_insert_policies_lib
77+
${TRAJECTORY_CACHE_DEPENDENCIES})
78+
3179
# Trajectory cache library
3280
add_library(moveit_ros_trajectory_cache_lib SHARED src/trajectory_cache.cpp)
3381
generate_export_header(moveit_ros_trajectory_cache_lib)
82+
target_link_libraries(
83+
moveit_ros_trajectory_cache_lib
84+
moveit_ros_trajectory_cache_cache_insert_policies_lib
85+
moveit_ros_trajectory_cache_features_lib
86+
moveit_ros_trajectory_cache_utils_lib)
3487
target_include_directories(
3588
moveit_ros_trajectory_cache_lib
3689
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
@@ -39,7 +92,7 @@ ament_target_dependencies(moveit_ros_trajectory_cache_lib
3992
${TRAJECTORY_CACHE_DEPENDENCIES})
4093

4194
install(
42-
TARGETS moveit_ros_trajectory_cache_lib
95+
TARGETS ${TRAJECTORY_CACHE_LIBRARIES}
4396
EXPORT moveit_ros_trajectory_cacheTargets
4497
LIBRARY DESTINATION lib
4598
ARCHIVE DESTINATION lib
@@ -48,29 +101,14 @@ install(
48101
DESTINATION include/moveit_ros_trajectory_cache)
49102

50103
install(DIRECTORY include/ DESTINATION include/moveit_ros_trajectory_cache)
51-
install(
52-
FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_ros_trajectory_cache_lib_export.h
53-
DESTINATION include/moveit_ros_trajectory_cache)
54-
55-
if(BUILD_TESTING)
56-
find_package(ament_cmake_pytest REQUIRED)
57-
find_package(launch_testing_ament_cmake REQUIRED)
58-
find_package(rmf_utils REQUIRED)
59-
find_package(warehouse_ros_sqlite REQUIRED)
60-
61-
# This test executable is run by the pytest_test, since a node is required for
62-
# testing move groups
63-
add_executable(test_trajectory_cache test/test_trajectory_cache.cpp)
64-
target_link_libraries(test_trajectory_cache moveit_ros_trajectory_cache_lib)
65-
ament_target_dependencies(test_trajectory_cache warehouse_ros_sqlite)
66-
67-
install(TARGETS test_trajectory_cache RUNTIME DESTINATION lib/${PROJECT_NAME})
68104

69-
ament_add_pytest_test(
70-
test_trajectory_cache_py "test/test_trajectory_cache.py" WORKING_DIRECTORY
71-
"${CMAKE_CURRENT_BINARY_DIR}")
105+
# Install export headers for each library
106+
foreach(lib_target ${TRAJECTORY_CACHE_LIBRARIES})
107+
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${lib_target}_export.h
108+
DESTINATION include/moveit_ros_trajectory_cache)
109+
endforeach()
72110

73-
endif()
111+
add_subdirectory(test)
74112

75113
ament_export_targets(moveit_ros_trajectory_cacheTargets HAS_LIBRARY_TARGET)
76114
ament_export_dependencies(${TRAJECTORY_CACHE_DEPENDENCIES})

0 commit comments

Comments
 (0)