From 3403b96d605a607f603f677f62c02ef9b99e33fd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sat, 30 Mar 2019 23:28:32 +0100 Subject: [PATCH 01/24] Port constraint_samplers to ROS 2 --- .../constraint_samplers/CMakeLists.txt | 61 +++++++++-------- .../constraint_samplers/constraint_sampler.h | 2 +- .../constraint_sampler_allocator.h | 1 + .../constraint_sampler_manager.h | 3 + .../constraint_sampler_tools.h | 7 +- .../default_constraint_samplers.h | 3 +- .../union_constraint_sampler.h | 1 + .../src/constraint_sampler.cpp | 4 +- .../src/constraint_sampler_manager.cpp | 34 +++++----- .../src/constraint_sampler_tools.cpp | 31 +++++---- .../src/default_constraint_samplers.cpp | 52 +++++++------- .../src/union_constraint_sampler.cpp | 8 ++- .../constraint_samplers/test/pr2_arm_ik.cpp | 17 +++-- .../constraint_samplers/test/pr2_arm_ik.h | 8 +-- .../test/pr2_arm_kinematics_plugin.cpp | 67 ++++++++++--------- .../test/pr2_arm_kinematics_plugin.h | 22 +++--- .../test/test_constraint_samplers.cpp | 16 ++--- 17 files changed, 186 insertions(+), 151 deletions(-) diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index 611552dbca..4d3042133b 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -9,40 +9,43 @@ 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) install(TARGETS ${MOVEIT_LIB_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) - -if(CATKIN_ENABLE_TESTING) - find_package(orocos_kdl REQUIRED) - find_package(angles REQUIRED) - find_package(tf2_kdl REQUIRED) - find_package(moveit_resources REQUIRED) + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) -include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS}) +install(DIRECTORY include/ DESTINATION include) - catkin_add_gtest(test_constraint_samplers - test/test_constraint_samplers.cpp - test/pr2_arm_kinematics_plugin.cpp - test/pr2_arm_ik.cpp - ) - target_link_libraries(test_constraint_samplers - moveit_test_utils - ${MOVEIT_LIB_NAME} - ${catkin_LIBRARIES} - ${angles_LIBRARIES} - ${orocos_kdl_LIBRARIES} - ${urdfdom_LIBRARIES} - ${urdfdom_headers_LIBRARIES} - ) -endif() +# 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}) +# +# ament_add_gtest(test_constraint_samplers +# test/test_constraint_samplers.cpp +# test/pr2_arm_kinematics_plugin.cpp +# test/pr2_arm_ik.cpp +# ) +# ament_target_dependencies(test_constraint_samplers +# moveit_test_utils +# ${MOVEIT_LIB_NAME} +# angles +# orcos_kdl +# urdfdom +# urdfdom_headers +# ) +# +# endif() diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h index 8dedddc74a..ccc1e1eef8 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h @@ -40,7 +40,7 @@ #include #include #include - +#include "rclcpp/rclcpp.hpp" /** * \brief The constraint samplers namespace contains a number of * methods for generating samples based on a constraint or set of diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h index 6da11dc42d..2beaf1e7c1 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h @@ -39,6 +39,7 @@ #include #include +#include "rclcpp/rclcpp.hpp" namespace constraint_samplers { diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h index ce71762c8b..8718da0e3f 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h @@ -39,6 +39,9 @@ #include #include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" namespace constraint_samplers { diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h index ff0ed4e81c..f1783f3d24 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h @@ -38,17 +38,18 @@ #define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_TOOLS_ #include -#include +#include +#include "rclcpp/rclcpp.hpp" namespace constraint_samplers { void visualizeDistribution(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state, const std::string& link_name, unsigned int sample_count, - visualization_msgs::MarkerArray& markers); + visualization_msgs::msg::MarkerArray& markers); void visualizeDistribution(const moveit_msgs::msg::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, const std::string& group, const std::string& link_name, unsigned int sample_count, - visualization_msgs::MarkerArray& markers); + visualization_msgs::msg::MarkerArray& markers); double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state); diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h index ca59e47c88..691cb382ae 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h @@ -40,6 +40,7 @@ #include #include #include +#include "rclcpp/rclcpp.hpp" namespace constraint_samplers { @@ -507,7 +508,7 @@ class IKConstraintSampler : public ConstraintSampler * * @return True if IK returns successfully with the timeout, and otherwise false. */ - bool callIK(const geometry_msgs::Pose& ik_query, + bool callIK(const geometry_msgs::msg::Pose& ik_query, const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, double timeout, robot_state::RobotState& state, bool use_as_seed); bool sampleHelper(robot_state::RobotState& state, const robot_state::RobotState& reference_state, diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h index e8d3abae81..832d73a89b 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h @@ -38,6 +38,7 @@ #define MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_UNION_CONSTRAINT_SAMPLER_ #include +#include "rclcpp/rclcpp.hpp" namespace constraint_samplers { diff --git a/moveit_core/constraint_samplers/src/constraint_sampler.cpp b/moveit_core/constraint_samplers/src/constraint_sampler.cpp index ea409909bf..9331f740fa 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler.cpp @@ -36,13 +36,15 @@ #include +rclcpp::Logger logger = rclcpp::get_logger("constraint_samplers"); + 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, "A JointModelGroup should have been specified for the constraint sampler"); } } diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index 26647e923b..da8969f3b2 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -39,6 +39,8 @@ #include #include +rclcpp::Logger logger = rclcpp::get_logger("constraint_samplers"); + constraint_samplers::ConstraintSamplerPtr constraint_samplers::ConstraintSamplerManager::selectSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, @@ -61,8 +63,8 @@ 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; + RCLCPP_DEBUG(logger, "Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n", jmg->getName().c_str(), ss.str().c_str()); @@ -70,7 +72,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // 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, "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, "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, "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, "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, "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, "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, "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, "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, "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, "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, "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, "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, "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, "No constraints sampler allocated for group '%s'", jmg->getName().c_str()); return ConstraintSamplerPtr(); } diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index 9167db532e..6894b75f61 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -37,10 +37,12 @@ #include #include +rclcpp::Logger logger = rclcpp::get_logger("constraint_samplers"); + void constraint_samplers::visualizeDistribution(const moveit_msgs::msg::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, const std::string& group, const std::string& link_name, - unsigned int sample_count, visualization_msgs::MarkerArray& markers) + unsigned int sample_count, visualization_msgs::msg::MarkerArray& markers) { visualizeDistribution(ConstraintSamplerManager::selectDefaultSampler(scene, group, constr), scene->getCurrentState(), link_name, sample_count, markers); @@ -59,13 +61,17 @@ double constraint_samplers::countSamplesPerSecond(const ConstraintSamplerPtr& sa { if (!sampler) { - ROS_ERROR_NAMED("constraint_samplers", "No sampler specified for counting samples per second"); + RCLCPP_ERROR(logger, "No sampler specified for counting samples per second"); return 0.0; } robot_state::RobotState ks(reference_state); unsigned long int valid = 0; unsigned long int total = 0; - ros::WallTime end = ros::WallTime::now() + ros::WallDuration(1.0); + //TODO RCLCPP::WallTimer has not a .now() function, and there is no wallDuration (they use std::chrono::nanoseconds) + // I'll use rclcpp::time, but this is something to fix. + rclcpp::Duration duration(1,0); + rclcpp::Time end = rclcpp::Clock().now() + duration; + // ros::WallTime end = ros::WallTime::now() + ros::WallDuration(1.0); do { static const unsigned int N = 10; @@ -75,25 +81,26 @@ double constraint_samplers::countSamplesPerSecond(const ConstraintSamplerPtr& sa if (sampler->sample(ks, 1)) valid++; } - } while (ros::WallTime::now() < end); + } while (rclcpp::Clock().now() < end); return (double)valid / (double)total; } void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state, const std::string& link_name, unsigned int sample_count, - visualization_msgs::MarkerArray& markers) + visualization_msgs::msg::MarkerArray& markers) { if (!sampler) { - ROS_ERROR_NAMED("constraint_samplers", "No sampler specified for visualizing distribution of samples"); + RCLCPP_ERROR(logger, "No sampler specified for visualizing distribution of samples"); return; } const robot_state::LinkModel* lm = reference_state.getLinkModel(link_name); if (!lm) return; robot_state::RobotState ks(reference_state); - std_msgs::ColorRGBA color; + rclcpp::Time stamp = rclcpp::Clock().now(); + std_msgs::msg::ColorRGBA color; color.r = 1.0f; color.g = 0.0f; color.b = 0.0f; @@ -103,20 +110,20 @@ void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& samp if (!sampler->sample(ks)) continue; const Eigen::Vector3d& pos = ks.getGlobalLinkTransform(lm).translation(); - visualization_msgs::Marker mk; - mk.header.stamp = ros::Time::now(); + visualization_msgs::msg::Marker mk; + mk.header.stamp = stamp; mk.header.frame_id = sampler->getJointModelGroup()->getParentModel().getModelFrame(); mk.ns = "constraint_samples"; mk.id = i; - mk.type = visualization_msgs::Marker::SPHERE; - mk.action = visualization_msgs::Marker::ADD; + mk.type = visualization_msgs::msg::Marker::SPHERE; + mk.action = visualization_msgs::msg::Marker::ADD; mk.pose.position.x = pos.x(); mk.pose.position.y = pos.y(); mk.pose.position.z = pos.z(); mk.pose.orientation.w = 1.0; mk.scale.x = mk.scale.y = mk.scale.z = 0.035; mk.color = color; - mk.lifetime = ros::Duration(30.0); + mk.lifetime = rclcpp::Duration(30.0); markers.markers.push_back(mk); } } diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 14bd6b00ff..d13547ba58 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -39,6 +39,8 @@ #include #include +rclcpp::Logger logger = rclcpp::get_logger("constraint_samplers"); + namespace constraint_samplers { bool JointConstraintSampler::configure(const moveit_msgs::msg::Constraints& constr) @@ -61,7 +63,7 @@ bool JointConstraintSampler::configure(const std::vector ji.max_bound_ + std::numeric_limits::epsilon()) { std::stringstream cs; jc[i].print(cs); - ROS_ERROR_NAMED("constraint_samplers", + RCLCPP_ERROR(logger, "The constraints for joint '%s' are such that " "there are no possible values for the joint: min_bound: %g, max_bound: %g. Failing.\n", jm->getName().c_str(), ji.min_bound_, ji.max_bound_); @@ -110,7 +112,7 @@ bool JointConstraintSampler::configure(const std::vectorenabled() && !sp.orientation_constraint_->enabled())) { - ROS_WARN_NAMED("constraint_samplers", "No enabled constraints in sampling pose"); + RCLCPP_WARN(logger, "No enabled constraints in sampling pose"); return false; } @@ -255,7 +257,7 @@ bool IKConstraintSampler::configure(const IKSamplingPose& sp) if (sampling_pose_.position_constraint_->getLinkModel()->getName() != sampling_pose_.orientation_constraint_->getLinkModel()->getName()) { - ROS_ERROR_NAMED("constraint_samplers", + RCLCPP_ERROR(logger, "Position and orientation constraints need to be specified for the same link " "in order to use IK-based sampling"); return false; @@ -268,7 +270,7 @@ bool IKConstraintSampler::configure(const IKSamplingPose& sp) kb_ = jmg_->getSolverInstance(); if (!kb_) { - ROS_WARN_NAMED("constraint_samplers", "No solver instance in setup"); + RCLCPP_WARN(logger, "No solver instance in setup"); is_valid_ = false; return false; } @@ -340,7 +342,7 @@ bool IKConstraintSampler::loadIKSolver() { if (!kb_) { - ROS_ERROR_NAMED("constraint_samplers", "No IK solver"); + RCLCPP_ERROR(logger, "No IK solver"); return false; } @@ -352,7 +354,7 @@ bool IKConstraintSampler::loadIKSolver() if (transform_ik_) if (!jmg_->getParentModel().hasLinkModel(ik_frame_)) { - ROS_ERROR_NAMED("constraint_samplers", + RCLCPP_ERROR(logger, "The IK solver expects requests in frame '%s' but this frame is not known to the sampler. " "Ignoring transformation (IK may fail)", ik_frame_.c_str()); @@ -399,7 +401,7 @@ bool IKConstraintSampler::loadIKSolver() if (wrong_link) { - ROS_ERROR_NAMED("constraint_samplers", + RCLCPP_ERROR(logger, "IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.", sampling_pose_.position_constraint_ ? sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() : @@ -416,7 +418,7 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q if (ks.dirtyLinkTransforms()) { // samplePose below requires accurate transforms - ROS_ERROR_NAMED("constraint_samplers", + RCLCPP_ERROR(logger, "IKConstraintSampler received dirty robot state, but valid transforms are required. " "Failing."); return false; @@ -437,13 +439,13 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q } if (!found) { - ROS_ERROR_NAMED("constraint_samplers", "Unable to sample a point inside the constraint region"); + RCLCPP_ERROR(logger, "Unable to sample a point inside the constraint region"); return false; } } else { - ROS_ERROR_NAMED("constraint_samplers", "Unable to sample a point inside the constraint region. " + RCLCPP_ERROR(logger, "Unable to sample a point inside the constraint region. " "Constraint region is empty when it should not be."); return false; } @@ -508,7 +510,7 @@ namespace { void samplingIkCallbackFnAdapter(robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, const robot_state::GroupStateValidityCallbackFn& constraint, - const geometry_msgs::Pose& /*unused*/, const std::vector& ik_sol, + const geometry_msgs::msg::Pose& /*unused*/, const std::vector& ik_sol, moveit_msgs::msg::MoveItErrorCodes& error_code) { const std::vector& bij = jmg->getKinematicsSolverJointBijection(); @@ -533,7 +535,7 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob { if (!is_valid_) { - ROS_WARN_NAMED("constraint_samplers", "IKConstraintSampler not configured, won't sample"); + RCLCPP_WARN(logger, "IKConstraintSampler not configured, won't sample"); return false; } @@ -550,7 +552,7 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob if (!samplePose(point, quat, reference_state, max_attempts)) { if (verbose_) - ROS_INFO_NAMED("constraint_samplers", "IK constraint sampler was unable to produce a pose to run IK for"); + RCLCPP_INFO(logger, "IK constraint sampler was unable to produce a pose to run IK for"); return false; } @@ -574,7 +576,7 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob quat = Eigen::Quaterniond(ikq.rotation()); } - geometry_msgs::Pose ik_query; + geometry_msgs::msg::Pose ik_query; ik_query.position.x = point.x(); ik_query.position.y = point.y(); ik_query.position.z = point.z(); @@ -603,7 +605,7 @@ bool IKConstraintSampler::validate(robot_state::RobotState& state) const sampling_pose_.position_constraint_->decide(state, verbose_).satisfied); } -bool IKConstraintSampler::callIK(const geometry_msgs::Pose& ik_query, +bool IKConstraintSampler::callIK(const geometry_msgs::msg::Pose& ik_query, const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, double timeout, robot_state::RobotState& state, bool use_as_seed) { @@ -640,12 +642,14 @@ bool IKConstraintSampler::callIK(const geometry_msgs::Pose& ik_query, { if (error.val != moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION && error.val != moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE && - error.val != moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT) - ROS_ERROR_NAMED("constraint_samplers", "IK solver failed with error %d", error.val); - else if (verbose_) - ROS_INFO_NAMED("constraint_samplers", "IK failed"); + error.val != moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT){ + RCLCPP_ERROR(logger, "IK solver failed with error %d", error.val); + } + else if (verbose_){ + RCLCPP_INFO(logger, "IK failed"); + } } return false; } -} // end of namespace constraint_samplers \ No newline at end of file +} // end of namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp index 84dbc233f9..41c9b75aee 100644 --- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp @@ -40,6 +40,8 @@ namespace constraint_samplers { + rclcpp::Logger logger = rclcpp::get_logger("constraint_samplers"); + struct OrderSamplers { bool operator()(const ConstraintSamplerPtr& a, const ConstraintSamplerPtr& b) const @@ -80,7 +82,7 @@ struct OrderSamplers } if (b_depends_on_a && a_depends_on_b) { - ROS_WARN_NAMED("constraint_samplers", + RCLCPP_WARN(logger, "Circular frame dependency! " "Sampling will likely produce invalid results (sampling for groups '%s' and '%s')", a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str()); @@ -118,7 +120,7 @@ UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSce for (std::size_t j = 0; j < fd.size(); ++j) frame_depends_.push_back(fd[j]); - ROS_DEBUG_NAMED("constraint_samplers", "Union sampler for group '%s' includes sampler for group '%s'", + RCLCPP_DEBUG(logger, "Union sampler for group '%s' includes sampler for group '%s'", jmg_->getName().c_str(), samplers_[i]->getJointModelGroup()->getName().c_str()); } } @@ -161,4 +163,4 @@ bool UnionConstraintSampler::project(robot_state::RobotState& state, unsigned in return true; } -} // end of namespace constraint_samplers \ No newline at end of file +} // end of namespace constraint_samplers diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index 64391e1ac0..e007a16081 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -49,6 +49,8 @@ using namespace angles; using namespace pr2_arm_kinematics; +rclcpp::Logger logger = rclcpp::get_logger("pr2_arm_kinematics_plugin"); + PR2ArmIK::PR2ArmIK() { } @@ -65,10 +67,11 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& joint = robot_model.getJoint(link->parent_joint->name); if (!joint) { - if (link->parent_joint) - ROS_ERROR_NAMED("pr2_arm_kinematics_plugin", "Could not find joint: %s", link->parent_joint->name.c_str()); - else - ROS_ERROR_NAMED("pr2_arm_kinematics_plugin", "Link %s has no parent joint", link->name.c_str()); + if (link->parent_joint){ + RCLCPP_ERROR(logger, "Could not find joint: %s", link->parent_joint->name.c_str()); + }else{ + RCLCPP_ERROR(logger, "Link %s has no parent joint", link->name.c_str()); + } return false; } if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) @@ -76,7 +79,7 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& link_offset.push_back(link->parent_joint->parent_to_joint_origin_transform); angle_multipliers_.push_back(joint->axis.x * fabs(joint->axis.x) + joint->axis.y * fabs(joint->axis.y) + joint->axis.z * fabs(joint->axis.z)); - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, + RCLCPP_DEBUG(logger, "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, joint->axis.y, joint->axis.z); if (joint->type != urdf::Joint::CONTINUOUS) { @@ -96,7 +99,7 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& { min_angles_.push_back(0.0); max_angles_.push_back(0.0); - ROS_WARN_NAMED("pr2_arm_kinematics_plugin", "No joint limits or joint '%s'", joint->name.c_str()); + RCLCPP_WARN(logger, "No joint limits or joint '%s'", joint->name.c_str()); } } continuous_joint_.push_back(false); @@ -128,7 +131,7 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& if (num_joints != 7) { - ROS_ERROR_NAMED("pr2_arm_kinematics_plugin", "PR2ArmIK:: Chain from %s to %s does not have 7 joints", + RCLCPP_ERROR(logger, "PR2ArmIK:: Chain from %s to %s does not have 7 joints", root_name.c_str(), tip_name.c_str()); return false; } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.h b/moveit_core/constraint_samplers/test/pr2_arm_ik.h index 4f023b5c16..d6f09cbea4 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.h @@ -42,10 +42,10 @@ #include #include // provides LU decomposition #include -#include -#include -#include - +#include +#include +#include +#include "rclcpp/rclcpp.hpp" namespace pr2_arm_kinematics { static const int NUM_JOINTS_ARM7DOF = 7; diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 64bcda303f..29c07d9193 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -34,7 +34,7 @@ /* Author: Sachin Chitta */ -#include +#include #include #include #include @@ -48,6 +48,9 @@ using namespace std; namespace pr2_arm_kinematics { + + rclcpp::Logger logger = rclcpp::get_logger("pr2_arm_kinematics_plugin"); + bool PR2ArmIKSolver::getCount(int& count, const int& max_count, const int& min_count) { if (count > 0) @@ -112,7 +115,7 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i if (free_angle_ == 0) { if (verbose) - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Solving with %f", q_init(0)); + RCLCPP_WARN(logger, "Solving with %f", q_init(0)); pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik); } else @@ -130,14 +133,14 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i { if (verbose) { - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Solution : %d", (int)solution_ik.size()); + RCLCPP_WARN(logger, "Solution : %d", (int)solution_ik.size()); for (int j = 0; j < (int)solution_ik[i].size(); j++) { - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%d: %f", j, solution_ik[i][j]); + RCLCPP_WARN(logger, "%d: %f", j, solution_ik[i][j]); } - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", " "); - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", " "); + RCLCPP_WARN(logger, " "); + RCLCPP_WARN(logger, " "); } double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init); if (tmp_distance < min_distance) @@ -167,7 +170,8 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& KDL::JntArray q_init = q_in; double initial_guess = q_init(free_angle_); - ros::Time start_time = ros::Time::now(); + rclcpp::Time start_time = rclcpp::Clock().now(); + rclcpp::Time time; double loop_time = 0; int count = 0; @@ -176,7 +180,7 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& int num_negative_increments = (int)((initial_guess - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position) / search_discretization_angle_); if (verbose) - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%f %f %f %d %d \n\n", initial_guess, + RCLCPP_WARN(logger, "%f %f %f %d %d \n\n", initial_guess, pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments, num_negative_increments); @@ -188,17 +192,18 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& return -1; q_init(free_angle_) = initial_guess + search_discretization_angle_ * count; if (verbose) - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%d, %f", count, q_init(free_angle_)); - loop_time = (ros::Time::now() - start_time).toSec(); + RCLCPP_WARN(logger, "%d, %f", count, q_init(free_angle_)); + time = rclcpp::Clock().now(); + loop_time = time.seconds() - start_time.seconds(); } if (loop_time >= timeout) { - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "IK Timed out in %f seconds", timeout); + RCLCPP_WARN(logger, "IK Timed out in %f seconds", timeout); return TIMED_OUT; } else { - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "No IK solution was found"); + RCLCPP_WARN(logger, "No IK solution was found"); return NO_IK_SOLUTION; } return NO_IK_SOLUTION; @@ -211,12 +216,12 @@ bool getKDLChain(const urdf::ModelInterface& model, const std::string& root_name KDL::Tree tree; if (!kdl_parser::treeFromUrdfModel(model, tree)) { - ROS_ERROR("Could not initialize tree object"); + RCLCPP_ERROR(logger,"Could not initialize tree object"); return false; } if (!tree.getChain(root_name, tip_name, kdl_chain)) { - ROS_ERROR_STREAM("Could not initialize chain object for base " << root_name << " tip " << tip_name); + RCLCPP_ERROR(logger,"Could not initialize chain object for base %s tip %s", root_name , tip_name); return false; } return true; @@ -274,11 +279,11 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo std::string xml_string; dimension_ = 7; - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Loading KDL Tree"); + RCLCPP_WARN(logger, "Loading KDL Tree"); if (!getKDLChain(*robot_model.getURDF(), base_frame_, tip_frames_[0], kdl_chain_)) { active_ = false; - ROS_ERROR("Could not load kdl tree"); + RCLCPP_ERROR(logger,"Could not load kdl tree"); } jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); free_angle_ = 2; @@ -287,7 +292,7 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo search_discretization, free_angle_)); if (!pr2_arm_ik_solver_->active_) { - ROS_ERROR("Could not load ik"); + RCLCPP_ERROR(logger,"Could not load ik"); active_ = false; } else @@ -300,41 +305,41 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo { for (unsigned int i = 0; i < ik_solver_info_.joint_names.size(); i++) { - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics:: joint name: %s", + RCLCPP_WARN(logger, "PR2Kinematics:: joint name: %s", ik_solver_info_.joint_names[i].c_str()); } for (unsigned int i = 0; i < ik_solver_info_.link_names.size(); i++) { - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics can solve IK for %s", + RCLCPP_WARN(logger, "PR2Kinematics can solve IK for %s", ik_solver_info_.link_names[i].c_str()); } for (unsigned int i = 0; i < fk_solver_info_.link_names.size(); i++) { - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics can solve FK for %s", + RCLCPP_WARN(logger, "PR2Kinematics can solve FK for %s", fk_solver_info_.link_names[i].c_str()); } - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2KinematicsPlugin::active for %s", group_name.c_str()); + RCLCPP_WARN(logger, "PR2KinematicsPlugin::active for %s", group_name.c_str()); } active_ = true; } return active_; } -bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, +bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { return false; } -bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, +bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { if (!active_) { - ROS_ERROR("kinematics not active"); + RCLCPP_ERROR(logger,"kinematics not active"); error_code.val = error_code.PLANNING_FAILED; return false; } @@ -369,13 +374,13 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose } else { - ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "An IK solution could not be found"); + RCLCPP_WARN(logger, "An IK solution could not be found"); error_code.val = error_code.NO_IK_SOLUTION; return false; } } -bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, +bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limit, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, @@ -384,7 +389,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose return false; } -bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, +bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, @@ -393,7 +398,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose return false; } -bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, +bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limit, std::vector& solution, const IKCallbackFn& solution_callback, @@ -405,7 +410,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector& link_names, const std::vector& joint_angles, - std::vector& poses) const + std::vector& poses) const { return false; } @@ -414,7 +419,7 @@ const std::vector& PR2ArmKinematicsPlugin::getJointNames() const { if (!active_) { - ROS_ERROR("kinematics not active"); + RCLCPP_ERROR(logger,"kinematics not active"); } return ik_solver_info_.joint_names; } @@ -423,7 +428,7 @@ const std::vector& PR2ArmKinematicsPlugin::getLinkNames() const { if (!active_) { - ROS_ERROR("kinematics not active"); + RCLCPP_ERROR(logger,"kinematics not active"); } return fk_solver_info_.link_names; } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index d535728ecb..e00e4ef9b9 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -46,10 +46,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -92,7 +92,7 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos #if KDL_VERSION_LESS(1, 4, 0) void updateInternalDataStructures(); #else - void updateInternalDataStructures() override; + void updateInternalDataStructures(); #endif #undef KDL_VERSION_LESS @@ -153,7 +153,7 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase * @return True if a valid solution was found, false otherwise */ bool getPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; @@ -166,7 +166,7 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase * @return True if a valid solution was found, false otherwise */ bool searchPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; /** @@ -179,7 +179,7 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase * @return True if a valid solution was found, false otherwise */ bool searchPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limits, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; @@ -193,7 +193,7 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase * @return True if a valid solution was found, false otherwise */ bool searchPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; @@ -208,7 +208,7 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase * @return True if a valid solution was found, false otherwise */ bool searchPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; @@ -221,7 +221,7 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase * @return True if a valid solution was found, false otherwise */ bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, - std::vector& poses) const override; + std::vector& poses) const override; /** * @brief Initialization function for the kinematics diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index 4978017bc8..b311d61479 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -44,7 +44,7 @@ #include #include -#include +#include #include #include @@ -263,7 +263,7 @@ TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerSimple) pcm.target_point_offset.y = 0; pcm.target_point_offset.z = 0; pcm.constraint_region.primitives.resize(1); - pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; + pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE; pcm.constraint_region.primitives[0].dimensions.resize(1); pcm.constraint_region.primitives[0].dimensions[0] = 0.001; @@ -376,7 +376,7 @@ TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerValid) pcm.target_point_offset.y = 0; pcm.target_point_offset.z = 0; pcm.constraint_region.primitives.resize(1); - pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; + pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE; pcm.constraint_region.primitives[0].dimensions.resize(1); pcm.constraint_region.primitives[0].dimensions[0] = 0.001; @@ -476,7 +476,7 @@ TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler) pcm.target_point_offset.y = 0; pcm.target_point_offset.z = 0; pcm.constraint_region.primitives.resize(1); - pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; + pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE; pcm.constraint_region.primitives[0].dimensions.resize(1); pcm.constraint_region.primitives[0].dimensions[0] = 0.001; @@ -621,7 +621,7 @@ TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager) pcm.target_point_offset.y = 0; pcm.target_point_offset.z = 0; pcm.constraint_region.primitives.resize(1); - pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; + pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE; pcm.constraint_region.primitives[0].dimensions.resize(1); pcm.constraint_region.primitives[0].dimensions[0] = 0.001; @@ -761,7 +761,7 @@ TEST_F(LoadPlanningModelsPr2, JointVersusPoseConstraintSamplerManager) con.position_constraints[0].target_point_offset.y = 0; con.position_constraints[0].target_point_offset.z = 0; con.position_constraints[0].constraint_region.primitives.resize(1); - con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; + con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE; con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1); con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001; @@ -887,7 +887,7 @@ TEST_F(LoadPlanningModelsPr2, MixedJointAndIkSamplerManager) con.position_constraints[0].target_point_offset.y = 0; con.position_constraints[0].target_point_offset.z = 0; con.position_constraints[0].constraint_region.primitives.resize(1); - con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; + con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE; con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1); con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001; @@ -1036,7 +1036,7 @@ TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler) pcm.target_point_offset.z = 0; pcm.constraint_region.primitives.resize(1); - pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE; + pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE; pcm.constraint_region.primitives[0].dimensions.resize(1); pcm.constraint_region.primitives[0].dimensions[0] = 0.001; From abd451dda53b037594911389973a87d15aff5ba4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sat, 30 Mar 2019 23:33:12 +0100 Subject: [PATCH 02/24] constraint_samplers, adapt logger according to https://github.com/ros-planning/moveit2/issues/21 --- .../src/constraint_sampler.cpp | 4 +- .../src/constraint_sampler_manager.cpp | 32 ++++++------- .../src/constraint_sampler_tools.cpp | 6 +-- .../src/default_constraint_samplers.cpp | 38 +++++++-------- .../src/union_constraint_sampler.cpp | 6 +-- .../constraint_samplers/test/pr2_arm_ik.cpp | 12 ++--- .../test/pr2_arm_kinematics_plugin.cpp | 46 +++++++++---------- 7 files changed, 72 insertions(+), 72 deletions(-) diff --git a/moveit_core/constraint_samplers/src/constraint_sampler.cpp b/moveit_core/constraint_samplers/src/constraint_sampler.cpp index 9331f740fa..ee99ed6ec5 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler.cpp @@ -36,7 +36,7 @@ #include -rclcpp::Logger logger = rclcpp::get_logger("constraint_samplers"); +rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("constraint_samplers"); constraint_samplers::ConstraintSampler::ConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name) @@ -44,7 +44,7 @@ constraint_samplers::ConstraintSampler::ConstraintSampler(const planning_scene:: { if (!jmg_) { - RCLCPP_ERROR(logger, "A JointModelGroup should have been specified for the constraint sampler"); + RCLCPP_ERROR(LOGGER, "A JointModelGroup should have been specified for the constraint sampler"); } } diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index da8969f3b2..1f62f24d19 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -39,7 +39,7 @@ #include #include -rclcpp::Logger logger = rclcpp::get_logger("constraint_samplers"); +rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("constraint_samplers"); constraint_samplers::ConstraintSamplerPtr constraint_samplers::ConstraintSamplerManager::selectSampler(const planning_scene::PlanningSceneConstPtr& scene, @@ -64,7 +64,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni return constraint_samplers::ConstraintSamplerPtr(); std::stringstream ss; ss << constr.name; - RCLCPP_DEBUG(logger, + RCLCPP_DEBUG(LOGGER, "Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n", jmg->getName().c_str(), ss.str().c_str()); @@ -72,7 +72,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // if there are joint constraints, we could possibly get a sampler from those if (!constr.joint_constraints.empty()) { - RCLCPP_DEBUG(logger, "There are joint constraints specified. " + RCLCPP_DEBUG(LOGGER, "There are joint constraints specified. " "Attempting to construct a JointConstraintSampler for group '%s'", jmg->getName().c_str()); @@ -110,7 +110,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName())); if (sampler->configure(jc)) { - RCLCPP_DEBUG(logger, "Allocated a sampler satisfying joint constraints for group '%s'", + RCLCPP_DEBUG(LOGGER, "Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str()); return sampler; } @@ -123,7 +123,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName())); if (sampler->configure(jc)) { - RCLCPP_DEBUG(logger, + RCLCPP_DEBUG(LOGGER, "Temporary sampler satisfying joint constraints for group '%s' allocated. " "Looking for different types of constraints before returning though.", jmg->getName().c_str()); @@ -144,7 +144,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // should be used if (ik_alloc) { - RCLCPP_DEBUG(logger, "There is an IK allocator for '%s'. " + RCLCPP_DEBUG(LOGGER, "There is an IK allocator for '%s'. " "Checking for corresponding position and/or orientation constraints", jmg->getName().c_str()); @@ -179,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; - RCLCPP_DEBUG(logger, "Allocated an IK-based sampler for group '%s' " + RCLCPP_DEBUG(LOGGER, "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()); } @@ -211,7 +211,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni if (use) { used_l[constr.position_constraints[p].link_name] = iks; - RCLCPP_DEBUG(logger, "Allocated an IK-based sampler for group '%s' " + RCLCPP_DEBUG(LOGGER, "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()); } @@ -240,7 +240,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni if (use) { used_l[constr.orientation_constraints[o].link_name] = iks; - RCLCPP_DEBUG(logger, "Allocated an IK-based sampler for group '%s' " + RCLCPP_DEBUG(LOGGER, "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()); } @@ -260,7 +260,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni } else if (used_l.size() > 1) { - RCLCPP_DEBUG(logger, + RCLCPP_DEBUG(LOGGER, "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 @@ -292,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()) { - RCLCPP_DEBUG(logger, "There are IK allocators for subgroups of group '%s'. " + RCLCPP_DEBUG(LOGGER, "There are IK allocators for subgroups of group '%s'. " "Checking for corresponding position and/or orientation constraints", jmg->getName().c_str()); @@ -323,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()) { - RCLCPP_DEBUG(logger, "Attempting to construct a sampler for the '%s' subgroup of '%s'", + RCLCPP_DEBUG(LOGGER, "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) { - RCLCPP_DEBUG(logger, "Constructed a sampler for the joints corresponding to group '%s', " + RCLCPP_DEBUG(LOGGER, "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; @@ -338,7 +338,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni } if (some_sampler_valid) { - RCLCPP_DEBUG(logger, "Constructing sampler for group '%s' as a union of %zu samplers", + RCLCPP_DEBUG(LOGGER, "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)); } @@ -347,12 +347,12 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // if we've gotten here, just return joint sampler if (joint_sampler) { - RCLCPP_DEBUG(logger, "Allocated a sampler satisfying joint constraints for group '%s'", + RCLCPP_DEBUG(LOGGER, "Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str()); return joint_sampler; } - RCLCPP_DEBUG(logger, "No constraints sampler allocated for group '%s'", jmg->getName().c_str()); + RCLCPP_DEBUG(LOGGER, "No constraints sampler allocated for group '%s'", jmg->getName().c_str()); return ConstraintSamplerPtr(); } diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index 6894b75f61..d72c931028 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -37,7 +37,7 @@ #include #include -rclcpp::Logger logger = rclcpp::get_logger("constraint_samplers"); +rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("constraint_samplers"); void constraint_samplers::visualizeDistribution(const moveit_msgs::msg::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, @@ -61,7 +61,7 @@ double constraint_samplers::countSamplesPerSecond(const ConstraintSamplerPtr& sa { if (!sampler) { - RCLCPP_ERROR(logger, "No sampler specified for counting samples per second"); + RCLCPP_ERROR(LOGGER, "No sampler specified for counting samples per second"); return 0.0; } robot_state::RobotState ks(reference_state); @@ -92,7 +92,7 @@ void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& samp { if (!sampler) { - RCLCPP_ERROR(logger, "No sampler specified for visualizing distribution of samples"); + RCLCPP_ERROR(LOGGER, "No sampler specified for visualizing distribution of samples"); return; } const robot_state::LinkModel* lm = reference_state.getLinkModel(link_name); diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index d13547ba58..222d7936b4 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -39,7 +39,7 @@ #include #include -rclcpp::Logger logger = rclcpp::get_logger("constraint_samplers"); +rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("constraint_samplers"); namespace constraint_samplers { @@ -63,7 +63,7 @@ bool JointConstraintSampler::configure(const std::vector ji.max_bound_ + std::numeric_limits::epsilon()) { std::stringstream cs; jc[i].print(cs); - RCLCPP_ERROR(logger, + RCLCPP_ERROR(LOGGER, "The constraints for joint '%s' are such that " "there are no possible values for the joint: min_bound: %g, max_bound: %g. Failing.\n", jm->getName().c_str(), ji.min_bound_, ji.max_bound_); @@ -112,7 +112,7 @@ bool JointConstraintSampler::configure(const std::vectorenabled() && !sp.orientation_constraint_->enabled())) { - RCLCPP_WARN(logger, "No enabled constraints in sampling pose"); + RCLCPP_WARN(LOGGER, "No enabled constraints in sampling pose"); return false; } @@ -257,7 +257,7 @@ bool IKConstraintSampler::configure(const IKSamplingPose& sp) if (sampling_pose_.position_constraint_->getLinkModel()->getName() != sampling_pose_.orientation_constraint_->getLinkModel()->getName()) { - RCLCPP_ERROR(logger, + RCLCPP_ERROR(LOGGER, "Position and orientation constraints need to be specified for the same link " "in order to use IK-based sampling"); return false; @@ -270,7 +270,7 @@ bool IKConstraintSampler::configure(const IKSamplingPose& sp) kb_ = jmg_->getSolverInstance(); if (!kb_) { - RCLCPP_WARN(logger, "No solver instance in setup"); + RCLCPP_WARN(LOGGER, "No solver instance in setup"); is_valid_ = false; return false; } @@ -342,7 +342,7 @@ bool IKConstraintSampler::loadIKSolver() { if (!kb_) { - RCLCPP_ERROR(logger, "No IK solver"); + RCLCPP_ERROR(LOGGER, "No IK solver"); return false; } @@ -354,7 +354,7 @@ bool IKConstraintSampler::loadIKSolver() if (transform_ik_) if (!jmg_->getParentModel().hasLinkModel(ik_frame_)) { - RCLCPP_ERROR(logger, + RCLCPP_ERROR(LOGGER, "The IK solver expects requests in frame '%s' but this frame is not known to the sampler. " "Ignoring transformation (IK may fail)", ik_frame_.c_str()); @@ -401,7 +401,7 @@ bool IKConstraintSampler::loadIKSolver() if (wrong_link) { - RCLCPP_ERROR(logger, + RCLCPP_ERROR(LOGGER, "IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.", sampling_pose_.position_constraint_ ? sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() : @@ -418,7 +418,7 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q if (ks.dirtyLinkTransforms()) { // samplePose below requires accurate transforms - RCLCPP_ERROR(logger, + RCLCPP_ERROR(LOGGER, "IKConstraintSampler received dirty robot state, but valid transforms are required. " "Failing."); return false; @@ -439,13 +439,13 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q } if (!found) { - RCLCPP_ERROR(logger, "Unable to sample a point inside the constraint region"); + RCLCPP_ERROR(LOGGER, "Unable to sample a point inside the constraint region"); return false; } } else { - RCLCPP_ERROR(logger, "Unable to sample a point inside the constraint region. " + RCLCPP_ERROR(LOGGER, "Unable to sample a point inside the constraint region. " "Constraint region is empty when it should not be."); return false; } @@ -535,7 +535,7 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob { if (!is_valid_) { - RCLCPP_WARN(logger, "IKConstraintSampler not configured, won't sample"); + RCLCPP_WARN(LOGGER, "IKConstraintSampler not configured, won't sample"); return false; } @@ -552,7 +552,7 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob if (!samplePose(point, quat, reference_state, max_attempts)) { if (verbose_) - RCLCPP_INFO(logger, "IK constraint sampler was unable to produce a pose to run IK for"); + RCLCPP_INFO(LOGGER, "IK constraint sampler was unable to produce a pose to run IK for"); return false; } @@ -643,10 +643,10 @@ bool IKConstraintSampler::callIK(const geometry_msgs::msg::Pose& ik_query, if (error.val != moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION && error.val != moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE && error.val != moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT){ - RCLCPP_ERROR(logger, "IK solver failed with error %d", error.val); + RCLCPP_ERROR(LOGGER, "IK solver failed with error %d", error.val); } else if (verbose_){ - RCLCPP_INFO(logger, "IK failed"); + RCLCPP_INFO(LOGGER, "IK failed"); } } return false; diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp index 41c9b75aee..216f2f849e 100644 --- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp @@ -40,7 +40,7 @@ namespace constraint_samplers { - rclcpp::Logger logger = rclcpp::get_logger("constraint_samplers"); + rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("constraint_samplers"); struct OrderSamplers { @@ -82,7 +82,7 @@ struct OrderSamplers } if (b_depends_on_a && a_depends_on_b) { - RCLCPP_WARN(logger, + RCLCPP_WARN(LOGGER, "Circular frame dependency! " "Sampling will likely produce invalid results (sampling for groups '%s' and '%s')", a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str()); @@ -120,7 +120,7 @@ UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSce for (std::size_t j = 0; j < fd.size(); ++j) frame_depends_.push_back(fd[j]); - RCLCPP_DEBUG(logger, "Union sampler for group '%s' includes sampler for group '%s'", + RCLCPP_DEBUG(LOGGER, "Union sampler for group '%s' includes sampler for group '%s'", jmg_->getName().c_str(), samplers_[i]->getJointModelGroup()->getName().c_str()); } } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index e007a16081..06d4e1585e 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -49,7 +49,7 @@ using namespace angles; using namespace pr2_arm_kinematics; -rclcpp::Logger logger = rclcpp::get_logger("pr2_arm_kinematics_plugin"); +rclcpp::Logger LOGGER = rclcpp::get_logger("pr2_arm_kinematics_plugin"); PR2ArmIK::PR2ArmIK() { @@ -68,9 +68,9 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& if (!joint) { if (link->parent_joint){ - RCLCPP_ERROR(logger, "Could not find joint: %s", link->parent_joint->name.c_str()); + RCLCPP_ERROR(LOGGER, "Could not find joint: %s", link->parent_joint->name.c_str()); }else{ - RCLCPP_ERROR(logger, "Link %s has no parent joint", link->name.c_str()); + RCLCPP_ERROR(LOGGER, "Link %s has no parent joint", link->name.c_str()); } return false; } @@ -79,7 +79,7 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& link_offset.push_back(link->parent_joint->parent_to_joint_origin_transform); angle_multipliers_.push_back(joint->axis.x * fabs(joint->axis.x) + joint->axis.y * fabs(joint->axis.y) + joint->axis.z * fabs(joint->axis.z)); - RCLCPP_DEBUG(logger, "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, + RCLCPP_DEBUG(LOGGER, "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, joint->axis.y, joint->axis.z); if (joint->type != urdf::Joint::CONTINUOUS) { @@ -99,7 +99,7 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& { min_angles_.push_back(0.0); max_angles_.push_back(0.0); - RCLCPP_WARN(logger, "No joint limits or joint '%s'", joint->name.c_str()); + RCLCPP_WARN(LOGGER, "No joint limits or joint '%s'", joint->name.c_str()); } } continuous_joint_.push_back(false); @@ -131,7 +131,7 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& if (num_joints != 7) { - RCLCPP_ERROR(logger, "PR2ArmIK:: Chain from %s to %s does not have 7 joints", + RCLCPP_ERROR(LOGGER, "PR2ArmIK:: Chain from %s to %s does not have 7 joints", root_name.c_str(), tip_name.c_str()); return false; } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 29c07d9193..e33b98fce9 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -49,7 +49,7 @@ using namespace std; namespace pr2_arm_kinematics { - rclcpp::Logger logger = rclcpp::get_logger("pr2_arm_kinematics_plugin"); + rclcpp::Logger LOGGER = rclcpp::get_logger("pr2_arm_kinematics_plugin"); bool PR2ArmIKSolver::getCount(int& count, const int& max_count, const int& min_count) { @@ -115,7 +115,7 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i if (free_angle_ == 0) { if (verbose) - RCLCPP_WARN(logger, "Solving with %f", q_init(0)); + RCLCPP_WARN(LOGGER, "Solving with %f", q_init(0)); pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik); } else @@ -133,14 +133,14 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i { if (verbose) { - RCLCPP_WARN(logger, "Solution : %d", (int)solution_ik.size()); + RCLCPP_WARN(LOGGER, "Solution : %d", (int)solution_ik.size()); for (int j = 0; j < (int)solution_ik[i].size(); j++) { - RCLCPP_WARN(logger, "%d: %f", j, solution_ik[i][j]); + RCLCPP_WARN(LOGGER, "%d: %f", j, solution_ik[i][j]); } - RCLCPP_WARN(logger, " "); - RCLCPP_WARN(logger, " "); + RCLCPP_WARN(LOGGER, " "); + RCLCPP_WARN(LOGGER, " "); } double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init); if (tmp_distance < min_distance) @@ -180,7 +180,7 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& int num_negative_increments = (int)((initial_guess - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position) / search_discretization_angle_); if (verbose) - RCLCPP_WARN(logger, "%f %f %f %d %d \n\n", initial_guess, + RCLCPP_WARN(LOGGER, "%f %f %f %d %d \n\n", initial_guess, pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments, num_negative_increments); @@ -192,18 +192,18 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& return -1; q_init(free_angle_) = initial_guess + search_discretization_angle_ * count; if (verbose) - RCLCPP_WARN(logger, "%d, %f", count, q_init(free_angle_)); + RCLCPP_WARN(LOGGER, "%d, %f", count, q_init(free_angle_)); time = rclcpp::Clock().now(); loop_time = time.seconds() - start_time.seconds(); } if (loop_time >= timeout) { - RCLCPP_WARN(logger, "IK Timed out in %f seconds", timeout); + RCLCPP_WARN(LOGGER, "IK Timed out in %f seconds", timeout); return TIMED_OUT; } else { - RCLCPP_WARN(logger, "No IK solution was found"); + RCLCPP_WARN(LOGGER, "No IK solution was found"); return NO_IK_SOLUTION; } return NO_IK_SOLUTION; @@ -216,12 +216,12 @@ bool getKDLChain(const urdf::ModelInterface& model, const std::string& root_name KDL::Tree tree; if (!kdl_parser::treeFromUrdfModel(model, tree)) { - RCLCPP_ERROR(logger,"Could not initialize tree object"); + RCLCPP_ERROR(LOGGER,"Could not initialize tree object"); return false; } if (!tree.getChain(root_name, tip_name, kdl_chain)) { - RCLCPP_ERROR(logger,"Could not initialize chain object for base %s tip %s", root_name , tip_name); + RCLCPP_ERROR(LOGGER,"Could not initialize chain object for base %s tip %s", root_name , tip_name); return false; } return true; @@ -279,11 +279,11 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo std::string xml_string; dimension_ = 7; - RCLCPP_WARN(logger, "Loading KDL Tree"); + RCLCPP_WARN(LOGGER, "Loading KDL Tree"); if (!getKDLChain(*robot_model.getURDF(), base_frame_, tip_frames_[0], kdl_chain_)) { active_ = false; - RCLCPP_ERROR(logger,"Could not load kdl tree"); + RCLCPP_ERROR(LOGGER,"Could not load kdl tree"); } jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); free_angle_ = 2; @@ -292,7 +292,7 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo search_discretization, free_angle_)); if (!pr2_arm_ik_solver_->active_) { - RCLCPP_ERROR(logger,"Could not load ik"); + RCLCPP_ERROR(LOGGER,"Could not load ik"); active_ = false; } else @@ -305,20 +305,20 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo { for (unsigned int i = 0; i < ik_solver_info_.joint_names.size(); i++) { - RCLCPP_WARN(logger, "PR2Kinematics:: joint name: %s", + RCLCPP_WARN(LOGGER, "PR2Kinematics:: joint name: %s", ik_solver_info_.joint_names[i].c_str()); } for (unsigned int i = 0; i < ik_solver_info_.link_names.size(); i++) { - RCLCPP_WARN(logger, "PR2Kinematics can solve IK for %s", + RCLCPP_WARN(LOGGER, "PR2Kinematics can solve IK for %s", ik_solver_info_.link_names[i].c_str()); } for (unsigned int i = 0; i < fk_solver_info_.link_names.size(); i++) { - RCLCPP_WARN(logger, "PR2Kinematics can solve FK for %s", + RCLCPP_WARN(LOGGER, "PR2Kinematics can solve FK for %s", fk_solver_info_.link_names[i].c_str()); } - RCLCPP_WARN(logger, "PR2KinematicsPlugin::active for %s", group_name.c_str()); + RCLCPP_WARN(LOGGER, "PR2KinematicsPlugin::active for %s", group_name.c_str()); } active_ = true; } @@ -339,7 +339,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik { if (!active_) { - RCLCPP_ERROR(logger,"kinematics not active"); + RCLCPP_ERROR(LOGGER,"kinematics not active"); error_code.val = error_code.PLANNING_FAILED; return false; } @@ -374,7 +374,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik } else { - RCLCPP_WARN(logger, "An IK solution could not be found"); + RCLCPP_WARN(LOGGER, "An IK solution could not be found"); error_code.val = error_code.NO_IK_SOLUTION; return false; } @@ -419,7 +419,7 @@ const std::vector& PR2ArmKinematicsPlugin::getJointNames() const { if (!active_) { - RCLCPP_ERROR(logger,"kinematics not active"); + RCLCPP_ERROR(LOGGER,"kinematics not active"); } return ik_solver_info_.joint_names; } @@ -428,7 +428,7 @@ const std::vector& PR2ArmKinematicsPlugin::getLinkNames() const { if (!active_) { - RCLCPP_ERROR(logger,"kinematics not active"); + RCLCPP_ERROR(LOGGER,"kinematics not active"); } return fk_solver_info_.link_names; } From 8b3fdcc6f2d2b707de92cbe7ec7ba8f7e595ec6f Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 24 May 2019 12:37:55 +0200 Subject: [PATCH 03/24] moveit_core constraint-samplers - library as shared --- moveit_core/constraint_samplers/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index 4d3042133b..f27d360718 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -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 From c48a10af7f4ee5c4a0b8e7de7c07916e2ca1cc6e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 26 Apr 2019 10:09:16 +0200 Subject: [PATCH 04/24] Fixing constraint samplers tests --- .../constraint_samplers/CMakeLists.txt | 51 ++++++++++--------- .../test/test_constraint_samplers.cpp | 12 ++--- 2 files changed, 34 insertions(+), 29 deletions(-) diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index f27d360718..8ac35e08fb 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -26,26 +26,31 @@ install(TARGETS ${MOVEIT_LIB_NAME} install(DIRECTORY include/ DESTINATION include) -# 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}) -# -# ament_add_gtest(test_constraint_samplers -# test/test_constraint_samplers.cpp -# test/pr2_arm_kinematics_plugin.cpp -# test/pr2_arm_ik.cpp -# ) -# ament_target_dependencies(test_constraint_samplers -# moveit_test_utils -# ${MOVEIT_LIB_NAME} -# angles -# orcos_kdl -# urdfdom -# urdfdom_headers -# ) -# -# endif() +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}) + + 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 + ${geometry_msgs_INCLUDE_DIRS} + ) + + target_link_libraries(test_constraint_samplers + moveit_test_utils + ${MOVEIT_LIB_NAME} + ${angles_LIBRARIES} + ${orcos_kdl_LIBRARIES} + ${urdfdom_LIBRARIES} + ${urdfdom_headers_LIBRARIES} + ) + +endif() diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index b311d61479..0f45bd7efc 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -673,8 +673,8 @@ TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager) if (s->sample(ks, ks_const, 1)) succ++; } - ROS_INFO("Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf", - (double)succ / (double)NT); + // ROS_INFO("Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf", + // (double)succ / (double)NT); // add additional ocm with smaller volume ocm.absolute_x_axis_tolerance = 0.1; @@ -1105,14 +1105,14 @@ TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler) if (s->sample(ks, ks_const, 1)) succ++; } - ROS_INFO_NAMED("pr2_arm_kinematics_plugin", - "Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf", - (double)succ / (double)NT); + // ROS_INFO_NAMED("pr2_arm_kinematics_plugin", + // "Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf", + // (double)succ / (double)NT); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::Time::init(); + // ros::Time::init(); return RUN_ALL_TESTS(); } From a58c4d98f615e0ac36d9059094ca82c8ec5b0b3d Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 26 Apr 2019 12:35:25 +0200 Subject: [PATCH 05/24] Fixing constraint_samplers tests --- .../constraint_samplers/CMakeLists.txt | 25 ++++++++++++++++--- .../test/pr2_arm_kinematics_plugin.cpp | 6 +++-- 2 files changed, 26 insertions(+), 5 deletions(-) diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index 8ac35e08fb..77dbdf9a48 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -34,6 +34,12 @@ if(BUILD_TESTING) include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS}) + if(WIN32) + # set(append_library_dirs "$;$") + 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 @@ -45,12 +51,25 @@ if(BUILD_TESTING) ) 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} - ${angles_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() diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index e33b98fce9..2de1c929da 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -343,8 +343,10 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik error_code.val = error_code.PLANNING_FAILED; return false; } - KDL::Frame pose_desired; - tf2::fromMsg(ik_pose, pose_desired); + KDL::Frame pose_desired = KDL::Frame(KDL::Rotation::Quaternion(ik_pose.orientation.x, ik_pose.orientation.y, + ik_pose.orientation.z, ik_pose.orientation.w), + KDL::Vector(ik_pose.position.x, ik_pose.position.y, ik_pose.position.z)); + // tf2::fromMsg(ik_pose, pose_desired); // Do the IK KDL::JntArray jnt_pos_in; From e2e067ed59fcc5a31022b0777771849c336a681b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Fri, 17 May 2019 00:38:51 +0200 Subject: [PATCH 06/24] Fixes for building in OS X --- moveit_core/constraint_samplers/CMakeLists.txt | 6 ++++++ .../constraint_samplers/test/pr2_arm_kinematics_plugin.cpp | 4 ++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index 77dbdf9a48..328a58c5a7 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -19,6 +19,12 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} 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 lib diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 2de1c929da..02deb52fc2 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -216,12 +216,12 @@ bool getKDLChain(const urdf::ModelInterface& model, const std::string& root_name KDL::Tree tree; if (!kdl_parser::treeFromUrdfModel(model, tree)) { - RCLCPP_ERROR(LOGGER,"Could not initialize tree object"); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "Could not initialize tree object"); return false; } if (!tree.getChain(root_name, tip_name, kdl_chain)) { - RCLCPP_ERROR(LOGGER,"Could not initialize chain object for base %s tip %s", root_name , tip_name); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "Could not initialize chain object for base %s tip %s", root_name.c_str(), tip_name.c_str()); return false; } return true; From f9be4c1f67cecb9656f34294d2f372b1bfec6a45 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 24 May 2019 12:42:46 +0200 Subject: [PATCH 07/24] moveit_core constraint_samplers - fixing logger variables --- .../src/constraint_sampler.cpp | 4 +- .../src/constraint_sampler_manager.cpp | 32 +++++++------- .../src/constraint_sampler_tools.cpp | 6 +-- .../src/default_constraint_samplers.cpp | 38 ++++++++--------- .../src/union_constraint_sampler.cpp | 6 +-- .../constraint_samplers/test/pr2_arm_ik.cpp | 12 +++--- .../test/pr2_arm_kinematics_plugin.cpp | 42 +++++++++---------- 7 files changed, 70 insertions(+), 70 deletions(-) diff --git a/moveit_core/constraint_samplers/src/constraint_sampler.cpp b/moveit_core/constraint_samplers/src/constraint_sampler.cpp index ee99ed6ec5..5a1429b4f4 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler.cpp @@ -36,7 +36,7 @@ #include -rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("constraint_samplers"); +rclcpp::Logger LOGGER_CONSTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); constraint_samplers::ConstraintSampler::ConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name) @@ -44,7 +44,7 @@ constraint_samplers::ConstraintSampler::ConstraintSampler(const planning_scene:: { if (!jmg_) { - RCLCPP_ERROR(LOGGER, "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"); } } diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index 1f62f24d19..85518a09b0 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -39,7 +39,7 @@ #include #include -rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("constraint_samplers"); +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, @@ -64,7 +64,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni return constraint_samplers::ConstraintSamplerPtr(); std::stringstream ss; ss << constr.name; - RCLCPP_DEBUG(LOGGER, + 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()); @@ -72,7 +72,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // if there are joint constraints, we could possibly get a sampler from those if (!constr.joint_constraints.empty()) { - RCLCPP_DEBUG(LOGGER, "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()); @@ -110,7 +110,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName())); if (sampler->configure(jc)) { - RCLCPP_DEBUG(LOGGER, "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; } @@ -123,7 +123,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName())); if (sampler->configure(jc)) { - RCLCPP_DEBUG(LOGGER, + 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()); @@ -144,7 +144,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // should be used if (ik_alloc) { - RCLCPP_DEBUG(LOGGER, "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()); @@ -179,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; - RCLCPP_DEBUG(LOGGER, "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()); } @@ -211,7 +211,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni if (use) { used_l[constr.position_constraints[p].link_name] = iks; - RCLCPP_DEBUG(LOGGER, "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()); } @@ -240,7 +240,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni if (use) { used_l[constr.orientation_constraints[o].link_name] = iks; - RCLCPP_DEBUG(LOGGER, "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()); } @@ -260,7 +260,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni } else if (used_l.size() > 1) { - RCLCPP_DEBUG(LOGGER, + 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 @@ -292,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()) { - RCLCPP_DEBUG(LOGGER, "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()); @@ -323,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()) { - RCLCPP_DEBUG(LOGGER, "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) { - RCLCPP_DEBUG(LOGGER, "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; @@ -338,7 +338,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni } if (some_sampler_valid) { - RCLCPP_DEBUG(LOGGER, "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)); } @@ -347,12 +347,12 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // if we've gotten here, just return joint sampler if (joint_sampler) { - RCLCPP_DEBUG(LOGGER, "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; } - RCLCPP_DEBUG(LOGGER, "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(); } diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index d72c931028..b6f3e6c8bc 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -37,7 +37,7 @@ #include #include -rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("constraint_samplers"); +rclcpp::Logger LOGGER_CONTRAINT_SAMPLERS_TOOLS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); void constraint_samplers::visualizeDistribution(const moveit_msgs::msg::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, @@ -61,7 +61,7 @@ double constraint_samplers::countSamplesPerSecond(const ConstraintSamplerPtr& sa { if (!sampler) { - RCLCPP_ERROR(LOGGER, "No sampler specified for counting samples per second"); + RCLCPP_ERROR(LOGGER_CONTRAINT_SAMPLERS_TOOLS, "No sampler specified for counting samples per second"); return 0.0; } robot_state::RobotState ks(reference_state); @@ -92,7 +92,7 @@ void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& samp { if (!sampler) { - RCLCPP_ERROR(LOGGER, "No sampler specified for visualizing distribution of samples"); + RCLCPP_ERROR(LOGGER_CONTRAINT_SAMPLERS_TOOLS, "No sampler specified for visualizing distribution of samples"); return; } const robot_state::LinkModel* lm = reference_state.getLinkModel(link_name); diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 222d7936b4..2423b5c064 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -39,7 +39,7 @@ #include #include -rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("constraint_samplers"); +rclcpp::Logger LOGGER_DEFAULT_CONTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); namespace constraint_samplers { @@ -63,7 +63,7 @@ bool JointConstraintSampler::configure(const std::vector ji.max_bound_ + std::numeric_limits::epsilon()) { std::stringstream cs; jc[i].print(cs); - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "The constraints for joint '%s' are such that " "there are no possible values for the joint: min_bound: %g, max_bound: %g. Failing.\n", jm->getName().c_str(), ji.min_bound_, ji.max_bound_); @@ -112,7 +112,7 @@ bool JointConstraintSampler::configure(const std::vectorenabled() && !sp.orientation_constraint_->enabled())) { - RCLCPP_WARN(LOGGER, "No enabled constraints in sampling pose"); + RCLCPP_WARN(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "No enabled constraints in sampling pose"); return false; } @@ -257,7 +257,7 @@ bool IKConstraintSampler::configure(const IKSamplingPose& sp) if (sampling_pose_.position_constraint_->getLinkModel()->getName() != sampling_pose_.orientation_constraint_->getLinkModel()->getName()) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "Position and orientation constraints need to be specified for the same link " "in order to use IK-based sampling"); return false; @@ -270,7 +270,7 @@ bool IKConstraintSampler::configure(const IKSamplingPose& sp) kb_ = jmg_->getSolverInstance(); if (!kb_) { - RCLCPP_WARN(LOGGER, "No solver instance in setup"); + RCLCPP_WARN(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "No solver instance in setup"); is_valid_ = false; return false; } @@ -342,7 +342,7 @@ bool IKConstraintSampler::loadIKSolver() { if (!kb_) { - RCLCPP_ERROR(LOGGER, "No IK solver"); + RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "No IK solver"); return false; } @@ -354,7 +354,7 @@ bool IKConstraintSampler::loadIKSolver() if (transform_ik_) if (!jmg_->getParentModel().hasLinkModel(ik_frame_)) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "The IK solver expects requests in frame '%s' but this frame is not known to the sampler. " "Ignoring transformation (IK may fail)", ik_frame_.c_str()); @@ -401,7 +401,7 @@ bool IKConstraintSampler::loadIKSolver() if (wrong_link) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.", sampling_pose_.position_constraint_ ? sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() : @@ -418,7 +418,7 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q if (ks.dirtyLinkTransforms()) { // samplePose below requires accurate transforms - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "IKConstraintSampler received dirty robot state, but valid transforms are required. " "Failing."); return false; @@ -439,13 +439,13 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q } if (!found) { - RCLCPP_ERROR(LOGGER, "Unable to sample a point inside the constraint region"); + RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "Unable to sample a point inside the constraint region"); return false; } } else { - RCLCPP_ERROR(LOGGER, "Unable to sample a point inside the constraint region. " + RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "Unable to sample a point inside the constraint region. " "Constraint region is empty when it should not be."); return false; } @@ -535,7 +535,7 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob { if (!is_valid_) { - RCLCPP_WARN(LOGGER, "IKConstraintSampler not configured, won't sample"); + RCLCPP_WARN(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "IKConstraintSampler not configured, won't sample"); return false; } @@ -552,7 +552,7 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob if (!samplePose(point, quat, reference_state, max_attempts)) { if (verbose_) - RCLCPP_INFO(LOGGER, "IK constraint sampler was unable to produce a pose to run IK for"); + RCLCPP_INFO(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "IK constraint sampler was unable to produce a pose to run IK for"); return false; } @@ -643,10 +643,10 @@ bool IKConstraintSampler::callIK(const geometry_msgs::msg::Pose& ik_query, if (error.val != moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION && error.val != moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE && error.val != moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT){ - RCLCPP_ERROR(LOGGER, "IK solver failed with error %d", error.val); + RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "IK solver failed with error %d", error.val); } else if (verbose_){ - RCLCPP_INFO(LOGGER, "IK failed"); + RCLCPP_INFO(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "IK failed"); } } return false; diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp index 216f2f849e..7df0869487 100644 --- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp @@ -40,7 +40,7 @@ namespace constraint_samplers { - rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("constraint_samplers"); + rclcpp::Logger LOGGER_UNION_CONTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); struct OrderSamplers { @@ -82,7 +82,7 @@ struct OrderSamplers } if (b_depends_on_a && a_depends_on_b) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(LOGGER_UNION_CONTRAINT_SAMPLERS, "Circular frame dependency! " "Sampling will likely produce invalid results (sampling for groups '%s' and '%s')", a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str()); @@ -120,7 +120,7 @@ UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSce for (std::size_t j = 0; j < fd.size(); ++j) frame_depends_.push_back(fd[j]); - RCLCPP_DEBUG(LOGGER, "Union sampler for group '%s' includes sampler for group '%s'", + RCLCPP_DEBUG(LOGGER_UNION_CONTRAINT_SAMPLERS, "Union sampler for group '%s' includes sampler for group '%s'", jmg_->getName().c_str(), samplers_[i]->getJointModelGroup()->getName().c_str()); } } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index 06d4e1585e..76d910f0ac 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -49,7 +49,7 @@ using namespace angles; using namespace pr2_arm_kinematics; -rclcpp::Logger LOGGER = rclcpp::get_logger("pr2_arm_kinematics_plugin"); +rclcpp::Logger LOGGER_PR2_ARM_IK = rclcpp::get_logger("pr2_arm_kinematics_plugin"); PR2ArmIK::PR2ArmIK() { @@ -68,9 +68,9 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& if (!joint) { if (link->parent_joint){ - RCLCPP_ERROR(LOGGER, "Could not find joint: %s", link->parent_joint->name.c_str()); + RCLCPP_ERROR(LOGGER_PR2_ARM_IK, "Could not find joint: %s", link->parent_joint->name.c_str()); }else{ - RCLCPP_ERROR(LOGGER, "Link %s has no parent joint", link->name.c_str()); + RCLCPP_ERROR(LOGGER_PR2_ARM_IK, "Link %s has no parent joint", link->name.c_str()); } return false; } @@ -79,7 +79,7 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& link_offset.push_back(link->parent_joint->parent_to_joint_origin_transform); angle_multipliers_.push_back(joint->axis.x * fabs(joint->axis.x) + joint->axis.y * fabs(joint->axis.y) + joint->axis.z * fabs(joint->axis.z)); - RCLCPP_DEBUG(LOGGER, "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, + RCLCPP_DEBUG(LOGGER_PR2_ARM_IK, "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, joint->axis.y, joint->axis.z); if (joint->type != urdf::Joint::CONTINUOUS) { @@ -99,7 +99,7 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& { min_angles_.push_back(0.0); max_angles_.push_back(0.0); - RCLCPP_WARN(LOGGER, "No joint limits or joint '%s'", joint->name.c_str()); + RCLCPP_WARN(LOGGER_PR2_ARM_IK, "No joint limits or joint '%s'", joint->name.c_str()); } } continuous_joint_.push_back(false); @@ -131,7 +131,7 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& if (num_joints != 7) { - RCLCPP_ERROR(LOGGER, "PR2ArmIK:: Chain from %s to %s does not have 7 joints", + RCLCPP_ERROR(LOGGER_PR2_ARM_IK, "PR2ArmIK:: Chain from %s to %s does not have 7 joints", root_name.c_str(), tip_name.c_str()); return false; } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 02deb52fc2..8724acb1db 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -49,7 +49,7 @@ using namespace std; namespace pr2_arm_kinematics { - rclcpp::Logger LOGGER = rclcpp::get_logger("pr2_arm_kinematics_plugin"); + rclcpp::Logger LOGGER_PR2_ARM_KINEMATICS_PLUGIN = rclcpp::get_logger("pr2_arm_kinematics_plugin"); bool PR2ArmIKSolver::getCount(int& count, const int& max_count, const int& min_count) { @@ -115,7 +115,7 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i if (free_angle_ == 0) { if (verbose) - RCLCPP_WARN(LOGGER, "Solving with %f", q_init(0)); + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "Solving with %f", q_init(0)); pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik); } else @@ -133,14 +133,14 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i { if (verbose) { - RCLCPP_WARN(LOGGER, "Solution : %d", (int)solution_ik.size()); + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "Solution : %d", (int)solution_ik.size()); for (int j = 0; j < (int)solution_ik[i].size(); j++) { - RCLCPP_WARN(LOGGER, "%d: %f", j, solution_ik[i][j]); + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "%d: %f", j, solution_ik[i][j]); } - RCLCPP_WARN(LOGGER, " "); - RCLCPP_WARN(LOGGER, " "); + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, " "); + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, " "); } double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init); if (tmp_distance < min_distance) @@ -180,7 +180,7 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& int num_negative_increments = (int)((initial_guess - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position) / search_discretization_angle_); if (verbose) - RCLCPP_WARN(LOGGER, "%f %f %f %d %d \n\n", initial_guess, + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "%f %f %f %d %d \n\n", initial_guess, pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments, num_negative_increments); @@ -192,18 +192,18 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& return -1; q_init(free_angle_) = initial_guess + search_discretization_angle_ * count; if (verbose) - RCLCPP_WARN(LOGGER, "%d, %f", count, q_init(free_angle_)); + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "%d, %f", count, q_init(free_angle_)); time = rclcpp::Clock().now(); loop_time = time.seconds() - start_time.seconds(); } if (loop_time >= timeout) { - RCLCPP_WARN(LOGGER, "IK Timed out in %f seconds", timeout); + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "IK Timed out in %f seconds", timeout); return TIMED_OUT; } else { - RCLCPP_WARN(LOGGER, "No IK solution was found"); + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "No IK solution was found"); return NO_IK_SOLUTION; } return NO_IK_SOLUTION; @@ -279,11 +279,11 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo std::string xml_string; dimension_ = 7; - RCLCPP_WARN(LOGGER, "Loading KDL Tree"); + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "Loading KDL Tree"); if (!getKDLChain(*robot_model.getURDF(), base_frame_, tip_frames_[0], kdl_chain_)) { active_ = false; - RCLCPP_ERROR(LOGGER,"Could not load kdl tree"); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN,"Could not load kdl tree"); } jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); free_angle_ = 2; @@ -292,7 +292,7 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo search_discretization, free_angle_)); if (!pr2_arm_ik_solver_->active_) { - RCLCPP_ERROR(LOGGER,"Could not load ik"); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN,"Could not load ik"); active_ = false; } else @@ -305,20 +305,20 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo { for (unsigned int i = 0; i < ik_solver_info_.joint_names.size(); i++) { - RCLCPP_WARN(LOGGER, "PR2Kinematics:: joint name: %s", + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "PR2Kinematics:: joint name: %s", ik_solver_info_.joint_names[i].c_str()); } for (unsigned int i = 0; i < ik_solver_info_.link_names.size(); i++) { - RCLCPP_WARN(LOGGER, "PR2Kinematics can solve IK for %s", + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "PR2Kinematics can solve IK for %s", ik_solver_info_.link_names[i].c_str()); } for (unsigned int i = 0; i < fk_solver_info_.link_names.size(); i++) { - RCLCPP_WARN(LOGGER, "PR2Kinematics can solve FK for %s", + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "PR2Kinematics can solve FK for %s", fk_solver_info_.link_names[i].c_str()); } - RCLCPP_WARN(LOGGER, "PR2KinematicsPlugin::active for %s", group_name.c_str()); + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "PR2KinematicsPlugin::active for %s", group_name.c_str()); } active_ = true; } @@ -339,7 +339,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik { if (!active_) { - RCLCPP_ERROR(LOGGER,"kinematics not active"); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN,"kinematics not active"); error_code.val = error_code.PLANNING_FAILED; return false; } @@ -376,7 +376,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik } else { - RCLCPP_WARN(LOGGER, "An IK solution could not be found"); + RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "An IK solution could not be found"); error_code.val = error_code.NO_IK_SOLUTION; return false; } @@ -421,7 +421,7 @@ const std::vector& PR2ArmKinematicsPlugin::getJointNames() const { if (!active_) { - RCLCPP_ERROR(LOGGER,"kinematics not active"); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN,"kinematics not active"); } return ik_solver_info_.joint_names; } @@ -430,7 +430,7 @@ const std::vector& PR2ArmKinematicsPlugin::getLinkNames() const { if (!active_) { - RCLCPP_ERROR(LOGGER,"kinematics not active"); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN,"kinematics not active"); } return fk_solver_info_.link_names; } From e7aa18594250e8d2e7b01535816d3dbca87875dd Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 29 May 2019 23:51:56 +0200 Subject: [PATCH 08/24] moveit_core constraint_samples - Fix whitespace in CMakeLists.txt --- .../constraint_samplers/CMakeLists.txt | 64 +++++++++---------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index 328a58c5a7..5c8ea9d769 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -9,7 +9,6 @@ add_library(${MOVEIT_LIB_NAME} SHARED ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_robot_state moveit_kinematic_constraints @@ -18,17 +17,19 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} urdf urdfdom urdfdom_headers - visualization_msgs) - + visualization_msgs +) + target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_trajectory moveit_robot_state - moveit_kinematic_constraints) - + moveit_kinematic_constraints +) install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib) + ARCHIVE DESTINATION lib +) install(DIRECTORY include/ DESTINATION include) @@ -40,11 +41,11 @@ if(BUILD_TESTING) include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS}) - if(WIN32) - # set(append_library_dirs "$;$") - 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() + if(WIN32) + # set(append_library_dirs "$;$") + 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 @@ -52,30 +53,29 @@ if(BUILD_TESTING) test/pr2_arm_ik.cpp ) - target_include_directories(test_constraint_samplers PUBLIC - ${geometry_msgs_INCLUDE_DIRS} - ) + target_include_directories(test_constraint_samplers PUBLIC + ${geometry_msgs_INCLUDE_DIRS} + ) target_link_libraries(test_constraint_samplers - moveit_constraint_samplers - moveit_kinematic_constraints - moveit_planning_scene - moveit_test_utils - moveit_utils - moveit_robot_state + moveit_constraint_samplers + moveit_kinematic_constraints + moveit_planning_scene + moveit_test_utils + moveit_utils + moveit_robot_state ${MOVEIT_LIB_NAME} - ${angles_LIBRARIES} - ${urdf_LIBRARIES} - ${tf2_LIBRARIES} - ${tf2_kdl_LIBRARIES} - ${tf2_ros_LIBRARIES} - ${kdl_parser_LIBRARIES} - ${OCTOMAP_LIBRARIES} - ${orcos_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} - ${rclcpp_headers_LIBRARIES} - ${geometric_shapes_LIBRARIES} + ${urdfdom_headers_LIBRARIES} + ${rclcpp_headers_LIBRARIES} + ${geometric_shapes_LIBRARIES} ) - endif() From c8fb7c101161e7d23b1caf4baa2964c04f6f7ee8 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 13 Jun 2019 10:41:51 +0200 Subject: [PATCH 09/24] Fixing endtime to use it inline --- .../constraint_samplers/src/constraint_sampler_tools.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index b6f3e6c8bc..36beb283f2 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -67,11 +67,7 @@ double constraint_samplers::countSamplesPerSecond(const ConstraintSamplerPtr& sa robot_state::RobotState ks(reference_state); unsigned long int valid = 0; unsigned long int total = 0; - //TODO RCLCPP::WallTimer has not a .now() function, and there is no wallDuration (they use std::chrono::nanoseconds) - // I'll use rclcpp::time, but this is something to fix. - rclcpp::Duration duration(1,0); - rclcpp::Time end = rclcpp::Clock().now() + duration; - // ros::WallTime end = ros::WallTime::now() + ros::WallDuration(1.0); + rclcpp::Time end = rclcpp::Clock().now() + rclcpp::Duration(1.0); do { static const unsigned int N = 10; From 082b80f6633611ad8f8a02b44733b5e544f5d232 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 13 Jun 2019 10:44:49 +0200 Subject: [PATCH 10/24] fix passing clock.now to stamp --- .../constraint_samplers/src/constraint_sampler_tools.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index 36beb283f2..1041384447 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -95,7 +95,6 @@ void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& samp if (!lm) return; robot_state::RobotState ks(reference_state); - rclcpp::Time stamp = rclcpp::Clock().now(); std_msgs::msg::ColorRGBA color; color.r = 1.0f; color.g = 0.0f; @@ -107,7 +106,7 @@ void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& samp continue; const Eigen::Vector3d& pos = ks.getGlobalLinkTransform(lm).translation(); visualization_msgs::msg::Marker mk; - mk.header.stamp = stamp; + mk.header.stamp = rclcpp::Clock().now(); mk.header.frame_id = sampler->getJointModelGroup()->getParentModel().getModelFrame(); mk.ns = "constraint_samples"; mk.id = i; From a527755c9757ef9a9aa3902ceb45adc84fc9ceb7 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 13 Jun 2019 10:48:37 +0200 Subject: [PATCH 11/24] Fix loop_time --- .../constraint_samplers/test/pr2_arm_kinematics_plugin.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 8724acb1db..bca115b1a2 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -171,7 +171,6 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& double initial_guess = q_init(free_angle_); rclcpp::Time start_time = rclcpp::Clock().now(); - rclcpp::Time time; double loop_time = 0; int count = 0; @@ -193,8 +192,7 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& q_init(free_angle_) = initial_guess + search_discretization_angle_ * count; if (verbose) RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "%d, %f", count, q_init(free_angle_)); - time = rclcpp::Clock().now(); - loop_time = time.seconds() - start_time.seconds(); + loop_time = rclcpp::Clock().now().seconds() - start_time.seconds(); } if (loop_time >= timeout) { From c7a3a048015d0f6796336dbaebeee102c7595efe Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 13 Jun 2019 10:50:28 +0200 Subject: [PATCH 12/24] Reverting back override to pr2_arm_kinematics_plugin --- .../constraint_samplers/test/pr2_arm_kinematics_plugin.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index e00e4ef9b9..cd55d69685 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -92,7 +92,7 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos #if KDL_VERSION_LESS(1, 4, 0) void updateInternalDataStructures(); #else - void updateInternalDataStructures(); + void updateInternalDataStructures() override; #endif #undef KDL_VERSION_LESS From 02f0eb363369b8954d154a4cd91a3a9eb786d119 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 13 Jun 2019 11:14:34 +0200 Subject: [PATCH 13/24] Remove updateInternalDataStructures On ROS2 is not implemented, for melodic we can get it on http://docs.ros.org/melodic/api/orocos_kdl/html/classKDL_1_1ChainJntToJacSolver.html#a8c6fe8ec5cd73c65699143e4a5df9bef, looking into ros2 code, there is not similar function https://github.com/ros2/orocos_kinematics_dynamics/tree/ros2/orocos_kdl --- .../test/pr2_arm_kinematics_plugin.cpp | 6 ------ .../constraint_samplers/test/pr2_arm_kinematics_plugin.h | 8 -------- 2 files changed, 14 deletions(-) diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index bca115b1a2..b36ad73111 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -101,12 +101,6 @@ PR2ArmIKSolver::PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const st active_ = true; } -void PR2ArmIKSolver::updateInternalDataStructures() -{ - // TODO: move (re)allocation of any internal data structures here - // to react to changes in chain -} - int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out) { const bool verbose = false; diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index cd55d69685..63874b527e 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -87,14 +87,6 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos ~PR2ArmIKSolver() override{}; -// TODO: simplify after kinetic support is dropped -#define KDL_VERSION_LESS(a, b, c) ((KDL_VERSION) < ((a << 16) | (b << 8) | c)) -#if KDL_VERSION_LESS(1, 4, 0) - void updateInternalDataStructures(); -#else - void updateInternalDataStructures() override; -#endif -#undef KDL_VERSION_LESS /** * @brief The PR2 inverse kinematics solver From 1611306fdc5f8fcf2b415b3aab202a25db9ee351 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 13 Jun 2019 11:19:00 +0200 Subject: [PATCH 14/24] Move constraint_sampler logger to the appropriate namespace --- .../include/moveit/constraint_samplers/constraint_sampler.h | 2 +- moveit_core/constraint_samplers/src/constraint_sampler.cpp | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h index ccc1e1eef8..024cc7618a 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h @@ -52,7 +52,7 @@ namespace constraint_samplers { MOVEIT_CLASS_FORWARD(ConstraintSampler); - +static rclcpp::Logger LOGGER_CONSTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); /** * \brief ConstraintSampler is an abstract base class that allows the * sampling of a kinematic state for a particular group of a robot. diff --git a/moveit_core/constraint_samplers/src/constraint_sampler.cpp b/moveit_core/constraint_samplers/src/constraint_sampler.cpp index 5a1429b4f4..6031e57c86 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler.cpp @@ -36,8 +36,6 @@ #include -rclcpp::Logger LOGGER_CONSTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); - 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) From 206c27d78a6fc0b4407423222c45fe7ced14398a Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 13 Jun 2019 11:20:21 +0200 Subject: [PATCH 15/24] Moving default_contraint_sampler logger into the namespace --- .../constraint_samplers/src/default_constraint_samplers.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 2423b5c064..35db35d5b9 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -39,10 +39,9 @@ #include #include -rclcpp::Logger LOGGER_DEFAULT_CONTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); - namespace constraint_samplers { + static rclcpp::Logger LOGGER_DEFAULT_CONTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); bool JointConstraintSampler::configure(const moveit_msgs::msg::Constraints& constr) { // construct the constraints From 322d4525398f0d3a61338ea31cadf4dfb42f33bd Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 13 Jun 2019 11:23:45 +0200 Subject: [PATCH 16/24] clang-format --- .../constraint_sampler_tools.h | 9 +-- .../src/constraint_sampler_manager.cpp | 70 ++++++++++--------- .../src/constraint_sampler_tools.cpp | 3 +- 3 files changed, 45 insertions(+), 37 deletions(-) diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h index f1783f3d24..d7abebd7f8 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h @@ -47,14 +47,15 @@ void visualizeDistribution(const ConstraintSamplerPtr& sampler, const robot_stat const std::string& link_name, unsigned int sample_count, visualization_msgs::msg::MarkerArray& markers); -void visualizeDistribution(const moveit_msgs::msg::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, - const std::string& group, const std::string& link_name, unsigned int sample_count, +void visualizeDistribution(const moveit_msgs::msg::Constraints& constr, + const planning_scene::PlanningSceneConstPtr& scene, const std::string& group, + const std::string& link_name, unsigned int sample_count, visualization_msgs::msg::MarkerArray& markers); double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state); -double countSamplesPerSecond(const moveit_msgs::msg::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, - const std::string& group); +double countSamplesPerSecond(const moveit_msgs::msg::Constraints& constr, + const planning_scene::PlanningSceneConstPtr& scene, const std::string& group); } #endif diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index 85518a09b0..b7aac98300 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -65,16 +65,16 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni std::stringstream ss; ss << constr.name; 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()); + "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()) { RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "There are joint constraints specified. " - "Attempting to construct a JointConstraintSampler for group '%s'", - jmg->getName().c_str()); + "Attempting to construct a JointConstraintSampler for group '%s'", + jmg->getName().c_str()); std::map joint_coverage; for (std::size_t i = 0; i < jmg->getVariableNames().size(); ++i) @@ -110,8 +110,8 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName())); if (sampler->configure(jc)) { - RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Allocated a sampler satisfying joint constraints for group '%s'", - jmg->getName().c_str()); + RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, + "Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str()); return sampler; } } @@ -124,9 +124,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni if (sampler->configure(jc)) { 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()); + "Temporary sampler satisfying joint constraints for group '%s' allocated. " + "Looking for different types of constraints before returning though.", + jmg->getName().c_str()); joint_sampler = sampler; } } @@ -144,9 +144,10 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // should be used if (ik_alloc) { - 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()); + 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()); // keep track of which links we constrained std::map used_l; @@ -179,9 +180,10 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni { // assign the link to a new constraint sampler used_l[constr.position_constraints[p].link_name] = iks; - 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()); + 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()); } } } @@ -212,8 +214,8 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni { used_l[constr.position_constraints[p].link_name] = iks; 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()); + "satisfying position constraints on link '%s'", + jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str()); } } } @@ -241,8 +243,8 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni { used_l[constr.orientation_constraints[o].link_name] = iks; 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()); + "satisfying orientation constraints on link '%s'", + jmg->getName().c_str(), constr.orientation_constraints[o].link_name.c_str()); } } } @@ -261,8 +263,8 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni else if (used_l.size() > 1) { 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()); + "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 IKConstraintSamplerPtr iks = used_l.begin()->second; double msv = iks->getSamplingVolume(); @@ -292,9 +294,10 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni // we now check to see if we can use samplers from subgroups if (!ik_subgroup_alloc.empty()) { - 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()); + 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()); bool some_sampler_valid = false; @@ -323,14 +326,16 @@ 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()) { - 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()); + 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) { - 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()); + 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; samplers.push_back(cs); } @@ -339,7 +344,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni if (some_sampler_valid) { RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Constructing sampler for group '%s' as a union of %zu samplers", - jmg->getName().c_str(), samplers.size()); + jmg->getName().c_str(), samplers.size()); return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers)); } } @@ -348,11 +353,12 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni if (joint_sampler) { RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Allocated a sampler satisfying joint constraints for group '%s'", - jmg->getName().c_str()); + jmg->getName().c_str()); return joint_sampler; } - RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "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(); } diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index 1041384447..a4728f0ed9 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -42,7 +42,8 @@ rclcpp::Logger LOGGER_CONTRAINT_SAMPLERS_TOOLS = rclcpp::get_logger("moveit").ge void constraint_samplers::visualizeDistribution(const moveit_msgs::msg::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, const std::string& group, const std::string& link_name, - unsigned int sample_count, visualization_msgs::msg::MarkerArray& markers) + unsigned int sample_count, + visualization_msgs::msg::MarkerArray& markers) { visualizeDistribution(ConstraintSamplerManager::selectDefaultSampler(scene, group, constr), scene->getCurrentState(), link_name, sample_count, markers); From 05ad105ae4a5b23a1f660187d42790479924c911 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 13 Jun 2019 18:21:06 +0200 Subject: [PATCH 17/24] Update logger test_constraint_samplers --- .../test/test_constraint_samplers.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index 0f45bd7efc..ae7b6dea0b 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -673,8 +673,8 @@ TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager) if (s->sample(ks, ks_const, 1)) succ++; } - // ROS_INFO("Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf", - // (double)succ / (double)NT); + RCLCPP_INFO(rclcpp::get_logger("test_constraint_samplers"),"Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf", + (double)succ / (double)NT); // add additional ocm with smaller volume ocm.absolute_x_axis_tolerance = 0.1; @@ -1105,9 +1105,9 @@ TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler) if (s->sample(ks, ks_const, 1)) succ++; } - // ROS_INFO_NAMED("pr2_arm_kinematics_plugin", - // "Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf", - // (double)succ / (double)NT); + RCLCPP_INFO(rclcpp::get_logger("pr2_arm_kinematics_plugin"), + "Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf", + (double)succ / (double)NT); } int main(int argc, char** argv) From 87bd1d5e0b7cd74c25c5d27768bbf5924da3deb5 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Fri, 14 Jun 2019 09:38:43 +0200 Subject: [PATCH 18/24] Constraint_sampler cmakelist formating --- moveit_core/constraint_samplers/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index 5c8ea9d769..4188b5fee7 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -76,6 +76,6 @@ if(BUILD_TESTING) ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${rclcpp_headers_LIBRARIES} - ${geometric_shapes_LIBRARIES} + ${geometric_shapes_LIBRARIES} ) endif() From a4c9c83a4e39c6e7aabe2ac6bfcfde57d47b5cf5 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Fri, 14 Jun 2019 09:40:39 +0200 Subject: [PATCH 19/24] Move constraint_sampler_tools logger to .hpp --- .../moveit/constraint_samplers/constraint_sampler_tools.h | 3 +++ .../constraint_samplers/src/constraint_sampler_tools.cpp | 2 -- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h index d7abebd7f8..450c92e593 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h @@ -43,6 +43,9 @@ namespace constraint_samplers { + + rclcpp::Logger LOGGER_CONTRAINT_SAMPLERS_TOOLS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); + void visualizeDistribution(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state, const std::string& link_name, unsigned int sample_count, visualization_msgs::msg::MarkerArray& markers); diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index a4728f0ed9..a1b1baa129 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -37,8 +37,6 @@ #include #include -rclcpp::Logger LOGGER_CONTRAINT_SAMPLERS_TOOLS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); - void constraint_samplers::visualizeDistribution(const moveit_msgs::msg::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene, const std::string& group, const std::string& link_name, From 23e2af614eab5f1ecc27f2299c40987d4200f183 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Fri, 14 Jun 2019 09:41:33 +0200 Subject: [PATCH 20/24] Using ros_time instead of rcl_system_time --- .../constraint_samplers/src/constraint_sampler_tools.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index a1b1baa129..457571b4e3 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -105,7 +105,7 @@ void constraint_samplers::visualizeDistribution(const ConstraintSamplerPtr& samp continue; const Eigen::Vector3d& pos = ks.getGlobalLinkTransform(lm).translation(); visualization_msgs::msg::Marker mk; - mk.header.stamp = rclcpp::Clock().now(); + mk.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now(); mk.header.frame_id = sampler->getJointModelGroup()->getParentModel().getModelFrame(); mk.ns = "constraint_samples"; mk.id = i; From d9872d1ad04537b9aa2617c438095fde9c524bf3 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Fri, 14 Jun 2019 09:43:23 +0200 Subject: [PATCH 21/24] Moving pr2_arm_ik logger to .h --- moveit_core/constraint_samplers/test/pr2_arm_ik.cpp | 2 -- moveit_core/constraint_samplers/test/pr2_arm_ik.h | 2 ++ 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index 76d910f0ac..2c1ed4535a 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -49,8 +49,6 @@ using namespace angles; using namespace pr2_arm_kinematics; -rclcpp::Logger LOGGER_PR2_ARM_IK = rclcpp::get_logger("pr2_arm_kinematics_plugin"); - PR2ArmIK::PR2ArmIK() { } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.h b/moveit_core/constraint_samplers/test/pr2_arm_ik.h index 0160694eba..220c15def7 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.h @@ -53,6 +53,8 @@ static const int NUM_JOINTS_ARM7DOF = 7; static const double IK_EPS = 1e-5; +rclcpp::Logger LOGGER_PR2_ARM_IK = rclcpp::get_logger("pr2_arm_kinematics_plugin"); + inline double distance(const urdf::Pose& transform) { return sqrt(transform.position.x * transform.position.x + transform.position.y * transform.position.y + From 01ca1b5d2768669845acffa9324e26fa89d13752 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Fri, 14 Jun 2019 09:44:56 +0200 Subject: [PATCH 22/24] adding RCL_ROS_TIME on pr2_arm_kinematics_plugin --- .../constraint_samplers/test/pr2_arm_kinematics_plugin.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index b36ad73111..fe1bc08d63 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -164,7 +164,7 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& KDL::JntArray q_init = q_in; double initial_guess = q_init(free_angle_); - rclcpp::Time start_time = rclcpp::Clock().now(); + rclcpp::Time start_time = rclcpp::Clock(RCL_ROS_TIME).now(); double loop_time = 0; int count = 0; @@ -186,7 +186,7 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& q_init(free_angle_) = initial_guess + search_discretization_angle_ * count; if (verbose) RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "%d, %f", count, q_init(free_angle_)); - loop_time = rclcpp::Clock().now().seconds() - start_time.seconds(); + loop_time = rclcpp::Clock(RCL_ROS_TIME).now().seconds() - start_time.seconds(); } if (loop_time >= timeout) { From 74f0720493328acbea9f512c2dbfb238e701602c Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Fri, 14 Jun 2019 09:45:57 +0200 Subject: [PATCH 23/24] removing dead code --- .../constraint_samplers/test/test_constraint_samplers.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index ae7b6dea0b..1c40dcdecc 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -1113,6 +1113,5 @@ TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler) int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - // ros::Time::init(); return RUN_ALL_TESTS(); } From abf4a74859bb5f3ca50a946b9bd0f48f1db59fa5 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Fri, 14 Jun 2019 09:48:54 +0200 Subject: [PATCH 24/24] clang format --- .../constraint_sampler_tools.h | 3 +- .../src/default_constraint_samplers.cpp | 45 ++++++++++--------- .../src/union_constraint_sampler.cpp | 10 ++--- .../constraint_samplers/test/pr2_arm_ik.cpp | 15 ++++--- .../test/pr2_arm_kinematics_plugin.cpp | 43 +++++++++--------- .../test/pr2_arm_kinematics_plugin.h | 4 +- .../test/test_constraint_samplers.cpp | 9 ++-- 7 files changed, 69 insertions(+), 60 deletions(-) diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h index 450c92e593..e325241fe3 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h @@ -43,8 +43,7 @@ namespace constraint_samplers { - - rclcpp::Logger LOGGER_CONTRAINT_SAMPLERS_TOOLS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); +rclcpp::Logger LOGGER_CONTRAINT_SAMPLERS_TOOLS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); void visualizeDistribution(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state, const std::string& link_name, unsigned int sample_count, diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 35db35d5b9..99ecb86acd 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -41,7 +41,7 @@ namespace constraint_samplers { - static rclcpp::Logger LOGGER_DEFAULT_CONTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); +static rclcpp::Logger LOGGER_DEFAULT_CONTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); bool JointConstraintSampler::configure(const moveit_msgs::msg::Constraints& constr) { // construct the constraints @@ -93,16 +93,16 @@ bool JointConstraintSampler::configure(const std::vector ji.max_bound_ + std::numeric_limits::epsilon()) { std::stringstream cs; jc[i].print(cs); RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, - "The constraints for joint '%s' are such that " - "there are no possible values for the joint: min_bound: %g, max_bound: %g. Failing.\n", - jm->getName().c_str(), ji.min_bound_, ji.max_bound_); + "The constraints for joint '%s' are such that " + "there are no possible values for the joint: min_bound: %g, max_bound: %g. Failing.\n", + jm->getName().c_str(), ji.min_bound_, ji.max_bound_); clear(); return false; } @@ -257,8 +257,8 @@ bool IKConstraintSampler::configure(const IKSamplingPose& sp) sampling_pose_.orientation_constraint_->getLinkModel()->getName()) { RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, - "Position and orientation constraints need to be specified for the same link " - "in order to use IK-based sampling"); + "Position and orientation constraints need to be specified for the same link " + "in order to use IK-based sampling"); return false; } @@ -354,9 +354,9 @@ bool IKConstraintSampler::loadIKSolver() if (!jmg_->getParentModel().hasLinkModel(ik_frame_)) { RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, - "The IK solver expects requests in frame '%s' but this frame is not known to the sampler. " - "Ignoring transformation (IK may fail)", - ik_frame_.c_str()); + "The IK solver expects requests in frame '%s' but this frame is not known to the sampler. " + "Ignoring transformation (IK may fail)", + ik_frame_.c_str()); transform_ik_ = false; } @@ -401,11 +401,11 @@ bool IKConstraintSampler::loadIKSolver() if (wrong_link) { RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, - "IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.", - sampling_pose_.position_constraint_ ? - sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() : - sampling_pose_.orientation_constraint_->getLinkModel()->getName().c_str(), - kb_->getTipFrame().c_str()); + "IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.", + sampling_pose_.position_constraint_ ? + sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() : + sampling_pose_.orientation_constraint_->getLinkModel()->getName().c_str(), + kb_->getTipFrame().c_str()); return false; } return true; @@ -418,8 +418,8 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q { // samplePose below requires accurate transforms RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, - "IKConstraintSampler received dirty robot state, but valid transforms are required. " - "Failing."); + "IKConstraintSampler received dirty robot state, but valid transforms are required. " + "Failing."); return false; } @@ -445,7 +445,7 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q else { RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "Unable to sample a point inside the constraint region. " - "Constraint region is empty when it should not be."); + "Constraint region is empty when it should not be."); return false; } @@ -551,7 +551,8 @@ bool IKConstraintSampler::sampleHelper(robot_state::RobotState& state, const rob if (!samplePose(point, quat, reference_state, max_attempts)) { if (verbose_) - RCLCPP_INFO(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "IK constraint sampler was unable to produce a pose to run IK for"); + RCLCPP_INFO(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, + "IK constraint sampler was unable to produce a pose to run IK for"); return false; } @@ -641,10 +642,12 @@ bool IKConstraintSampler::callIK(const geometry_msgs::msg::Pose& ik_query, { if (error.val != moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION && error.val != moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE && - error.val != moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT){ + error.val != moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT) + { RCLCPP_ERROR(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "IK solver failed with error %d", error.val); } - else if (verbose_){ + else if (verbose_) + { RCLCPP_INFO(LOGGER_DEFAULT_CONTRAINT_SAMPLERS, "IK failed"); } } diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp index 7df0869487..6ada22a6a9 100644 --- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp @@ -40,7 +40,7 @@ namespace constraint_samplers { - rclcpp::Logger LOGGER_UNION_CONTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); +rclcpp::Logger LOGGER_UNION_CONTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers"); struct OrderSamplers { @@ -83,9 +83,9 @@ struct OrderSamplers if (b_depends_on_a && a_depends_on_b) { RCLCPP_WARN(LOGGER_UNION_CONTRAINT_SAMPLERS, - "Circular frame dependency! " - "Sampling will likely produce invalid results (sampling for groups '%s' and '%s')", - a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str()); + "Circular frame dependency! " + "Sampling will likely produce invalid results (sampling for groups '%s' and '%s')", + a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str()); return true; } if (b_depends_on_a && !a_depends_on_b) @@ -121,7 +121,7 @@ UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSce frame_depends_.push_back(fd[j]); RCLCPP_DEBUG(LOGGER_UNION_CONTRAINT_SAMPLERS, "Union sampler for group '%s' includes sampler for group '%s'", - jmg_->getName().c_str(), samplers_[i]->getJointModelGroup()->getName().c_str()); + jmg_->getName().c_str(), samplers_[i]->getJointModelGroup()->getName().c_str()); } } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index 2c1ed4535a..ca3aadf2c7 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -65,9 +65,12 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& joint = robot_model.getJoint(link->parent_joint->name); if (!joint) { - if (link->parent_joint){ + if (link->parent_joint) + { RCLCPP_ERROR(LOGGER_PR2_ARM_IK, "Could not find joint: %s", link->parent_joint->name.c_str()); - }else{ + } + else + { RCLCPP_ERROR(LOGGER_PR2_ARM_IK, "Link %s has no parent joint", link->name.c_str()); } return false; @@ -77,8 +80,8 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& link_offset.push_back(link->parent_joint->parent_to_joint_origin_transform); angle_multipliers_.push_back(joint->axis.x * fabs(joint->axis.x) + joint->axis.y * fabs(joint->axis.y) + joint->axis.z * fabs(joint->axis.z)); - RCLCPP_DEBUG(LOGGER_PR2_ARM_IK, "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, - joint->axis.y, joint->axis.z); + RCLCPP_DEBUG(LOGGER_PR2_ARM_IK, "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, joint->axis.y, + joint->axis.z); if (joint->type != urdf::Joint::CONTINUOUS) { if (joint->safety) @@ -129,8 +132,8 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& if (num_joints != 7) { - RCLCPP_ERROR(LOGGER_PR2_ARM_IK, "PR2ArmIK:: Chain from %s to %s does not have 7 joints", - root_name.c_str(), tip_name.c_str()); + RCLCPP_ERROR(LOGGER_PR2_ARM_IK, "PR2ArmIK:: Chain from %s to %s does not have 7 joints", root_name.c_str(), + tip_name.c_str()); return false; } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index fe1bc08d63..ca899638b6 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -48,8 +48,7 @@ using namespace std; namespace pr2_arm_kinematics { - - rclcpp::Logger LOGGER_PR2_ARM_KINEMATICS_PLUGIN = rclcpp::get_logger("pr2_arm_kinematics_plugin"); +rclcpp::Logger LOGGER_PR2_ARM_KINEMATICS_PLUGIN = rclcpp::get_logger("pr2_arm_kinematics_plugin"); bool PR2ArmIKSolver::getCount(int& count, const int& max_count, const int& min_count) { @@ -174,9 +173,9 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& (int)((initial_guess - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position) / search_discretization_angle_); if (verbose) RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "%f %f %f %d %d \n\n", initial_guess, - pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments, - num_negative_increments); + pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, + pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments, + num_negative_increments); while (loop_time < timeout) { if (CartToJnt(q_init, p_in, q_out) > 0) @@ -213,7 +212,8 @@ bool getKDLChain(const urdf::ModelInterface& model, const std::string& root_name } if (!tree.getChain(root_name, tip_name, kdl_chain)) { - RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "Could not initialize chain object for base %s tip %s", root_name.c_str(), tip_name.c_str()); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "Could not initialize chain object for base %s tip %s", + root_name.c_str(), tip_name.c_str()); return false; } return true; @@ -275,7 +275,7 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo if (!getKDLChain(*robot_model.getURDF(), base_frame_, tip_frames_[0], kdl_chain_)) { active_ = false; - RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN,"Could not load kdl tree"); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "Could not load kdl tree"); } jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); free_angle_ = 2; @@ -284,7 +284,7 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo search_discretization, free_angle_)); if (!pr2_arm_ik_solver_->active_) { - RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN,"Could not load ik"); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "Could not load ik"); active_ = false; } else @@ -298,17 +298,17 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo for (unsigned int i = 0; i < ik_solver_info_.joint_names.size(); i++) { RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "PR2Kinematics:: joint name: %s", - ik_solver_info_.joint_names[i].c_str()); + ik_solver_info_.joint_names[i].c_str()); } for (unsigned int i = 0; i < ik_solver_info_.link_names.size(); i++) { RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "PR2Kinematics can solve IK for %s", - ik_solver_info_.link_names[i].c_str()); + ik_solver_info_.link_names[i].c_str()); } for (unsigned int i = 0; i < fk_solver_info_.link_names.size(); i++) { RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "PR2Kinematics can solve FK for %s", - fk_solver_info_.link_names[i].c_str()); + fk_solver_info_.link_names[i].c_str()); } RCLCPP_WARN(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "PR2KinematicsPlugin::active for %s", group_name.c_str()); } @@ -317,8 +317,9 @@ bool PR2ArmKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_mo return active_; } -bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, +bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, + const std::vector& ik_seed_state, std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { return false; @@ -326,18 +327,19 @@ bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_po bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { if (!active_) { - RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN,"kinematics not active"); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "kinematics not active"); error_code.val = error_code.PLANNING_FAILED; return false; } KDL::Frame pose_desired = KDL::Frame(KDL::Rotation::Quaternion(ik_pose.orientation.x, ik_pose.orientation.y, - ik_pose.orientation.z, ik_pose.orientation.w), - KDL::Vector(ik_pose.position.x, ik_pose.position.y, ik_pose.position.z)); + ik_pose.orientation.z, ik_pose.orientation.w), + KDL::Vector(ik_pose.position.x, ik_pose.position.y, ik_pose.position.z)); // tf2::fromMsg(ik_pose, pose_desired); // Do the IK @@ -377,7 +379,8 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limit, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { return false; @@ -413,7 +416,7 @@ const std::vector& PR2ArmKinematicsPlugin::getJointNames() const { if (!active_) { - RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN,"kinematics not active"); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "kinematics not active"); } return ik_solver_info_.joint_names; } @@ -422,7 +425,7 @@ const std::vector& PR2ArmKinematicsPlugin::getLinkNames() const { if (!active_) { - RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN,"kinematics not active"); + RCLCPP_ERROR(LOGGER_PR2_ARM_KINEMATICS_PLUGIN, "kinematics not active"); } return fk_solver_info_.link_names; } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index 63874b527e..7fc4f17335 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -87,7 +87,6 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos ~PR2ArmIKSolver() override{}; - /** * @brief The PR2 inverse kinematics solver */ @@ -186,7 +185,8 @@ class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase */ bool searchPositionIK( const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; /** diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp index 1c40dcdecc..8adc74d652 100644 --- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp @@ -673,8 +673,9 @@ TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager) if (s->sample(ks, ks_const, 1)) succ++; } - RCLCPP_INFO(rclcpp::get_logger("test_constraint_samplers"),"Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf", - (double)succ / (double)NT); + RCLCPP_INFO(rclcpp::get_logger("test_constraint_samplers"), + "Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf", + (double)succ / (double)NT); // add additional ocm with smaller volume ocm.absolute_x_axis_tolerance = 0.1; @@ -1106,8 +1107,8 @@ TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler) succ++; } RCLCPP_INFO(rclcpp::get_logger("pr2_arm_kinematics_plugin"), - "Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf", - (double)succ / (double)NT); + "Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf", + (double)succ / (double)NT); } int main(int argc, char** argv)