-
Notifications
You must be signed in to change notification settings - Fork 565
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Constraint samplers #60
Changes from 8 commits
3403b96
abd451d
8b3fdcc
c48a10a
a58c4d9
e2e067e
f9be4c1
d71434a
e7aa185
c8fb7c1
082b80f
a527755
c7a3a04
02f0eb3
1611306
206c27d
322d452
05ad105
87bd1d5
a4c9c83
23e2af6
d9872d1
01ca1b5
74f0720
abf4a74
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,6 +1,6 @@ | ||
set(MOVEIT_LIB_NAME moveit_constraint_samplers) | ||
|
||
add_library(${MOVEIT_LIB_NAME} | ||
add_library(${MOVEIT_LIB_NAME} SHARED | ||
src/constraint_sampler.cpp | ||
src/constraint_sampler_manager.cpp | ||
src/constraint_sampler_tools.cpp | ||
|
@@ -9,40 +9,73 @@ add_library(${MOVEIT_LIB_NAME} | |
) | ||
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") | ||
|
||
target_link_libraries(${MOVEIT_LIB_NAME} | ||
|
||
ament_target_dependencies(${MOVEIT_LIB_NAME} | ||
moveit_robot_state | ||
moveit_kinematic_constraints | ||
moveit_kinematics_base | ||
moveit_planning_scene | ||
${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) | ||
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) | ||
urdf | ||
urdfdom | ||
urdfdom_headers | ||
visualization_msgs) | ||
|
||
target_link_libraries(${MOVEIT_LIB_NAME} | ||
moveit_robot_trajectory | ||
moveit_robot_state | ||
moveit_kinematic_constraints) | ||
|
||
|
||
install(TARGETS ${MOVEIT_LIB_NAME} | ||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} | ||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) | ||
LIBRARY DESTINATION lib | ||
ARCHIVE DESTINATION lib) | ||
|
||
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) | ||
install(DIRECTORY include/ DESTINATION include) | ||
|
||
if(CATKIN_ENABLE_TESTING) | ||
if(BUILD_TESTING) | ||
find_package(orocos_kdl REQUIRED) | ||
find_package(angles REQUIRED) | ||
find_package(tf2_kdl REQUIRED) | ||
find_package(moveit_resources REQUIRED) | ||
|
||
include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS}) | ||
include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS}) | ||
|
||
catkin_add_gtest(test_constraint_samplers | ||
if(WIN32) | ||
# set(append_library_dirs "$<TARGET_FILE_DIR:${PROJECT_NAME}>;$<TARGET_FILE_DIR:${PROJECT_NAME}_TestPlugins1>") | ||
else() | ||
set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../planning_scene;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../kinematics_constraint") | ||
endif() | ||
|
||
ament_add_gtest(test_constraint_samplers | ||
test/test_constraint_samplers.cpp | ||
test/pr2_arm_kinematics_plugin.cpp | ||
test/pr2_arm_ik.cpp | ||
) | ||
|
||
target_include_directories(test_constraint_samplers PUBLIC | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. fix whitespace There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. |
||
${geometry_msgs_INCLUDE_DIRS} | ||
) | ||
|
||
target_link_libraries(test_constraint_samplers | ||
moveit_test_utils | ||
moveit_constraint_samplers | ||
moveit_kinematic_constraints | ||
moveit_planning_scene | ||
moveit_test_utils | ||
moveit_utils | ||
moveit_robot_state | ||
${MOVEIT_LIB_NAME} | ||
${catkin_LIBRARIES} | ||
${angles_LIBRARIES} | ||
${orocos_kdl_LIBRARIES} | ||
${angles_LIBRARIES} | ||
${urdf_LIBRARIES} | ||
${tf2_LIBRARIES} | ||
${tf2_kdl_LIBRARIES} | ||
${tf2_ros_LIBRARIES} | ||
${kdl_parser_LIBRARIES} | ||
${OCTOMAP_LIBRARIES} | ||
${orcos_kdl_LIBRARIES} | ||
${urdfdom_LIBRARIES} | ||
${urdfdom_headers_LIBRARIES} | ||
${urdfdom_headers_LIBRARIES} | ||
${rclcpp_headers_LIBRARIES} | ||
${geometric_shapes_LIBRARIES} | ||
) | ||
|
||
endif() |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -36,13 +36,15 @@ | |
|
||
#include <moveit/constraint_samplers/constraint_sampler.h> | ||
|
||
rclcpp::Logger LOGGER_CONSTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Refactor to put this inside namespace. the methods themselves could be put in a single large namespace block as well There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. |
||
|
||
constraint_samplers::ConstraintSampler::ConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, | ||
const std::string& group_name) | ||
: is_valid_(false), scene_(scene), jmg_(scene->getRobotModel()->getJointModelGroup(group_name)), verbose_(false) | ||
{ | ||
if (!jmg_) | ||
{ | ||
ROS_ERROR_NAMED("constraint_samplers", "A JointModelGroup should have been specified for the constraint sampler"); | ||
RCLCPP_ERROR(LOGGER_CONSTRAINT_SAMPLERS, "A JointModelGroup should have been specified for the constraint sampler"); | ||
} | ||
} | ||
|
||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -39,6 +39,8 @@ | |
#include <moveit/constraint_samplers/union_constraint_sampler.h> | ||
#include <sstream> | ||
|
||
rclcpp::Logger LOGGER_CONTRAINT_SAMPLERS_MANAGER = rclcpp::get_logger("moveit").get_child("constraint_samplers"); | ||
|
||
constraint_samplers::ConstraintSamplerPtr | ||
constraint_samplers::ConstraintSamplerManager::selectSampler(const planning_scene::PlanningSceneConstPtr& scene, | ||
const std::string& group_name, | ||
|
@@ -61,16 +63,16 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
if (!jmg) | ||
return constraint_samplers::ConstraintSamplerPtr(); | ||
std::stringstream ss; | ||
ss << constr; | ||
ROS_DEBUG_NAMED("constraint_samplers", | ||
ss << constr.name; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Isn't there a way to print the whole message as a string anymore? There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I still think that the full constraint should be printed here instead of only the name. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. There is no such implementation for ROS2. I have asked that on the following issue : ros2/common_interfaces#69 (comment) redirected to ros2/rosidl#259 |
||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, | ||
"Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n", | ||
jmg->getName().c_str(), ss.str().c_str()); | ||
|
||
ConstraintSamplerPtr joint_sampler; // location to put chosen joint sampler if needed | ||
// if there are joint constraints, we could possibly get a sampler from those | ||
if (!constr.joint_constraints.empty()) | ||
{ | ||
ROS_DEBUG_NAMED("constraint_samplers", "There are joint constraints specified. " | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "There are joint constraints specified. " | ||
"Attempting to construct a JointConstraintSampler for group '%s'", | ||
jmg->getName().c_str()); | ||
|
||
|
@@ -108,7 +110,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName())); | ||
if (sampler->configure(jc)) | ||
{ | ||
ROS_DEBUG_NAMED("constraint_samplers", "Allocated a sampler satisfying joint constraints for group '%s'", | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Allocated a sampler satisfying joint constraints for group '%s'", | ||
jmg->getName().c_str()); | ||
return sampler; | ||
} | ||
|
@@ -121,7 +123,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName())); | ||
if (sampler->configure(jc)) | ||
{ | ||
ROS_DEBUG_NAMED("constraint_samplers", | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, | ||
"Temporary sampler satisfying joint constraints for group '%s' allocated. " | ||
"Looking for different types of constraints before returning though.", | ||
jmg->getName().c_str()); | ||
|
@@ -142,7 +144,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
// should be used | ||
if (ik_alloc) | ||
{ | ||
ROS_DEBUG_NAMED("constraint_samplers", "There is an IK allocator for '%s'. " | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "There is an IK allocator for '%s'. " | ||
"Checking for corresponding position and/or orientation constraints", | ||
jmg->getName().c_str()); | ||
|
||
|
@@ -177,7 +179,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
{ | ||
// assign the link to a new constraint sampler | ||
used_l[constr.position_constraints[p].link_name] = iks; | ||
ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' " | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Allocated an IK-based sampler for group '%s' " | ||
"satisfying position and orientation constraints on link '%s'", | ||
jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str()); | ||
} | ||
|
@@ -209,7 +211,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
if (use) | ||
{ | ||
used_l[constr.position_constraints[p].link_name] = iks; | ||
ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' " | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Allocated an IK-based sampler for group '%s' " | ||
"satisfying position constraints on link '%s'", | ||
jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str()); | ||
} | ||
|
@@ -238,7 +240,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
if (use) | ||
{ | ||
used_l[constr.orientation_constraints[o].link_name] = iks; | ||
ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' " | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Allocated an IK-based sampler for group '%s' " | ||
"satisfying orientation constraints on link '%s'", | ||
jmg->getName().c_str(), constr.orientation_constraints[o].link_name.c_str()); | ||
} | ||
|
@@ -258,7 +260,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
} | ||
else if (used_l.size() > 1) | ||
{ | ||
ROS_DEBUG_NAMED("constraint_samplers", | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, | ||
"Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume", | ||
jmg->getName().c_str()); | ||
// find the sampler with the smallest sampling volume; delete the rest | ||
|
@@ -290,7 +292,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
// we now check to see if we can use samplers from subgroups | ||
if (!ik_subgroup_alloc.empty()) | ||
{ | ||
ROS_DEBUG_NAMED("constraint_samplers", "There are IK allocators for subgroups of group '%s'. " | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "There are IK allocators for subgroups of group '%s'. " | ||
"Checking for corresponding position and/or orientation constraints", | ||
jmg->getName().c_str()); | ||
|
||
|
@@ -321,12 +323,12 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
// if some matching constraints were found, construct the allocator | ||
if (!sub_constr.orientation_constraints.empty() || !sub_constr.position_constraints.empty()) | ||
{ | ||
ROS_DEBUG_NAMED("constraint_samplers", "Attempting to construct a sampler for the '%s' subgroup of '%s'", | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Attempting to construct a sampler for the '%s' subgroup of '%s'", | ||
it->first->getName().c_str(), jmg->getName().c_str()); | ||
ConstraintSamplerPtr cs = selectDefaultSampler(scene, it->first->getName(), sub_constr); | ||
if (cs) | ||
{ | ||
ROS_DEBUG_NAMED("constraint_samplers", "Constructed a sampler for the joints corresponding to group '%s', " | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Constructed a sampler for the joints corresponding to group '%s', " | ||
"but part of group '%s'", | ||
it->first->getName().c_str(), jmg->getName().c_str()); | ||
some_sampler_valid = true; | ||
|
@@ -336,7 +338,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
} | ||
if (some_sampler_valid) | ||
{ | ||
ROS_DEBUG_NAMED("constraint_samplers", "Constructing sampler for group '%s' as a union of %zu samplers", | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Constructing sampler for group '%s' as a union of %zu samplers", | ||
jmg->getName().c_str(), samplers.size()); | ||
return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers)); | ||
} | ||
|
@@ -345,12 +347,12 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni | |
// if we've gotten here, just return joint sampler | ||
if (joint_sampler) | ||
{ | ||
ROS_DEBUG_NAMED("constraint_samplers", "Allocated a sampler satisfying joint constraints for group '%s'", | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Allocated a sampler satisfying joint constraints for group '%s'", | ||
jmg->getName().c_str()); | ||
return joint_sampler; | ||
} | ||
|
||
ROS_DEBUG_NAMED("constraint_samplers", "No constraints sampler allocated for group '%s'", jmg->getName().c_str()); | ||
RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "No constraints sampler allocated for group '%s'", jmg->getName().c_str()); | ||
|
||
return ConstraintSamplerPtr(); | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Hardcoding relative paths is a bad idea.
I would recommend that for every library in this package that is used else ware you set a
<lib_name>_lib_dir
environment variable that can then be used by the testsA good example is the
rcl_lib_dir
I have taken the time to show how it is used below:The example uses rcl_add_custom_gtest but it should work just as well with ament_add_gtest
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If you don't mind I prefer to merge this. And then I will fix them all together. Because we have other merged that use this style. I can open an issue to avoid forgetting it.