diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt index 88deb403dc..7a1f4739e2 100644 --- a/moveit_core/collision_detection/CMakeLists.txt +++ b/moveit_core/collision_detection/CMakeLists.txt @@ -11,7 +11,19 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/world.cpp src/world_diff.cpp ) + set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) +ament_target_dependencies(${MOVEIT_LIB_NAME} + moveit_robot_state + rclcpp + rmw_implementation + urdf + urdfdom + urdfdom_headers + visualization_msgs + tf2_eigen + geometric_shapes +) ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_robot_state diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index e56b0d5ce8..7a2ca2bf9a 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -40,7 +40,6 @@ #include #include #include -#include #include #include #include @@ -70,7 +69,7 @@ enum Type /** \brief Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is * CONDITIONAL) */ -typedef boost::function DecideContactFn; +typedef std::function DecideContactFn; MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index ea45aa72f0..d7fa3f2c05 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -1,25 +1,61 @@ set(MOVEIT_LIB_NAME moveit_kinematic_constraints) -add_library(${MOVEIT_LIB_NAME} +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_model;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl") +endif() + +add_library(${MOVEIT_LIB_NAME} SHARED src/kinematic_constraint.cpp - src/utils.cpp) + src/utils.cpp +) + set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(${MOVEIT_LIB_NAME} + urdf + urdfdom + urdfdom_headers + tf2_geometry_msgs + visualization_msgs + tf2_eigen +) + target_link_libraries(${MOVEIT_LIB_NAME} - moveit_robot_model moveit_kinematics_base moveit_robot_state moveit_collision_detection_fcl moveit_utils - ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) + moveit_collision_detection_fcl + moveit_kinematics_base + moveit_robot_state + moveit_robot_model +) install(TARGETS ${MOVEIT_LIB_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +install(DIRECTORY include/ DESTINATION include) -if(CATKIN_ENABLE_TESTING) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) find_package(moveit_resources REQUIRED) + find_package(resource_retriever REQUIRED) + include_directories(${moveit_resources_INCLUDE_DIRS}) - catkin_add_gtest(test_constraints test/test_constraints.cpp) - target_link_libraries(test_constraints moveit_test_utils ${MOVEIT_LIB_NAME}) + ament_add_gtest(test_constraints test/test_constraints.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + ) + + target_link_libraries(test_constraints + moveit_kinematic_constraints + moveit_collision_detection_fcl + moveit_robot_model + moveit_robot_state + moveit_utils + moveit_test_utils + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever + ${MOVEIT_LIB_NAME} + ) endif() diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index aff84b5218..a1cac551b1 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -49,6 +49,10 @@ #include #include +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/duration.hpp" + /** \brief Representation and evaluation of kinematic constraints */ namespace kinematic_constraints { @@ -814,7 +818,7 @@ class VisibilityConstraint : public KinematicConstraint * @param [in] state The state from which to produce the markers * @param [out] markers The marker array to which the markers will be added */ - void getMarkers(const robot_state::RobotState& state, visualization_msgs::MarkerArray& markers) const; + void getMarkers(const robot_state::RobotState& state, visualization_msgs::msg::MarkerArray& markers) const; bool enabled() const override; ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override; diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h index 5509c147ab..490dde72e1 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -38,9 +38,9 @@ #define MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_ #include -#include -#include -#include +#include +#include +#include #include #include @@ -118,7 +118,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const robot_state::RobotS * * @return A full constraint message containing both constraints */ -moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose, +moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::PoseStamped& pose, double tolerance_pos = 1e-3, double tolerance_angle = 1e-2); /** @@ -136,7 +136,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n * * @return A full constraint message containing both constraints */ -moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose, +moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::PoseStamped& pose, const std::vector& tolerance_pos, const std::vector& tolerance_angle); @@ -152,7 +152,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n * @return A full constraint message containing the orientation constraint */ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, - const geometry_msgs::QuaternionStamped& quat, + const geometry_msgs::msg::QuaternionStamped& quat, double tolerance = 1e-2); /** @@ -169,8 +169,8 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n * @return A full constraint message containing the position constraint */ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, - const geometry_msgs::Point& reference_point, - const geometry_msgs::PointStamped& goal_point, + const geometry_msgs::msg::Point& reference_point, + const geometry_msgs::msg::PointStamped& goal_point, double tolerance = 1e-3); /** @@ -186,7 +186,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n * @return A full constraint message containing the position constraint */ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, - const geometry_msgs::PointStamped& goal_point, + const geometry_msgs::msg::PointStamped& goal_point, double tolerance = 1e-3); /** @@ -200,7 +200,8 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n * * @return was the construction successful? */ -bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints); + // TODO rework this function +// bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints); } #endif diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 9852926f99..0f815a1bb7 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -45,9 +45,12 @@ #include #include #include - +#include namespace kinematic_constraints { + +rclcpp::Logger LOGGER_KINEMATIC_CONSTRAINTS = rclcpp::get_logger("kinematic_constraints"); + static double normalizeAngle(double angle) { double v = fmod(angle, 2.0 * boost::math::constants::pi()); @@ -73,7 +76,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) // testing tolerances first if (jc.tolerance_above < 0.0 || jc.tolerance_below < 0.0) { - ROS_WARN_NAMED("kinematic_constraints", "JointConstraint tolerance values must be positive."); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "JointConstraint tolerance values must be positive."); joint_model_ = nullptr; return false; } @@ -102,12 +105,12 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) // check if the joint has 1 DOF (the only kind we can handle) if (joint_model_->getVariableCount() == 0) { - ROS_ERROR_NAMED("kinematic_constraints", "Joint '%s' has no parameters to constrain", jc.joint_name.c_str()); + RCLCPP_ERROR(LOGGER_KINEMATIC_CONSTRAINTS, "Joint '%s' has no parameters to constrain", jc.joint_name.c_str()); joint_model_ = nullptr; } else if (joint_model_->getVariableCount() > 1) { - ROS_ERROR_NAMED("kinematic_constraints", "Joint '%s' has more than one parameter to constrain. " + RCLCPP_ERROR(LOGGER_KINEMATIC_CONSTRAINTS, "Joint '%s' has more than one parameter to constrain. " "This type of constraint is not supported.", jc.joint_name.c_str()); joint_model_ = nullptr; @@ -125,7 +128,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) } if (found < 0) { - ROS_ERROR_NAMED("kinematic_constraints", "Local variable name '%s' is not known to joint '%s'", + RCLCPP_ERROR(LOGGER_KINEMATIC_CONSTRAINTS, "Local variable name '%s' is not known to joint '%s'", local_variable_name_.c_str(), joint_model_->getName().c_str()); joint_model_ = nullptr; } @@ -166,7 +169,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) { joint_position_ = bounds.min_position_; joint_tolerance_above_ = std::numeric_limits::epsilon(); - ROS_WARN_NAMED("kinematic_constraints", "Joint %s is constrained to be below the minimum bounds. " + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Joint %s is constrained to be below the minimum bounds. " "Assuming minimum bounds instead.", jc.joint_name.c_str()); } @@ -174,7 +177,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) { joint_position_ = bounds.max_position_; joint_tolerance_below_ = std::numeric_limits::epsilon(); - ROS_WARN_NAMED("kinematic_constraints", "Joint %s is constrained to be above the maximum bounds. " + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Joint %s is constrained to be above the maximum bounds. " "Assuming maximum bounds instead.", jc.joint_name.c_str()); } @@ -182,7 +185,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) if (jc.weight <= std::numeric_limits::epsilon()) { - ROS_WARN_NAMED("kinematic_constraints", + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "The weight on constraint for joint '%s' is very near zero. Setting to 1.0.", jc.joint_name.c_str()); constraint_weight_ = 1.0; @@ -230,7 +233,7 @@ ConstraintEvaluationResult JointConstraint::decide(const robot_state::RobotState bool result = dif <= (joint_tolerance_above_ + 2.0 * std::numeric_limits::epsilon()) && dif >= (-joint_tolerance_below_ - 2.0 * std::numeric_limits::epsilon()); if (verbose) - ROS_INFO_NAMED("kinematic_constraints", "Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, " + RCLCPP_INFO(LOGGER_KINEMATIC_CONSTRAINTS, "Constraint %s:: Joint name: '%s', actual value: %f, desired value: %f, " "tolerance_above: %f, tolerance_below: %f", result ? "satisfied" : "violated", joint_variable_name_.c_str(), current_joint_position, joint_position_, joint_tolerance_above_, joint_tolerance_below_); @@ -277,7 +280,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p link_model_ = robot_model_->getLinkModel(pc.link_name); if (link_model_ == nullptr) { - ROS_WARN_NAMED("kinematic_constraints", + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Position constraint link model %s not found in kinematic model. Constraint invalid.", pc.link_name.c_str()); return false; @@ -285,7 +288,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p if (pc.header.frame_id.empty()) { - ROS_WARN_NAMED("kinematic_constraints", "No frame specified for position constraint on link '%s'!", + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "No frame specified for position constraint on link '%s'!", pc.link_name.c_str()); return false; } @@ -312,7 +315,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p { if (pc.constraint_region.primitive_poses.size() <= i) { - ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses"); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Constraint region message does not contain enough primitive poses"); continue; } constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get()))); @@ -328,7 +331,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p } } else - ROS_WARN_NAMED("kinematic_constraints", "Could not construct primitive shape %zu", i); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Could not construct primitive shape %zu", i); } // load meshes @@ -339,7 +342,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p { if (pc.constraint_region.mesh_poses.size() <= i) { - ROS_WARN_NAMED("kinematic_constraints", "Constraint region message does not contain enough primitive poses"); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Constraint region message does not contain enough primitive poses"); continue; } constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get()))); @@ -356,13 +359,13 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p } else { - ROS_WARN_NAMED("kinematic_constraints", "Could not construct mesh shape %zu", i); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Could not construct mesh shape %zu", i); } } if (pc.weight <= std::numeric_limits::epsilon()) { - ROS_WARN_NAMED("kinematic_constraints", + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "The weight on position constraint for link '%s' is near zero. Setting to 1.0.", pc.link_name.c_str()); constraint_weight_ = 1.0; @@ -422,10 +425,10 @@ static inline ConstraintEvaluationResult finishPositionConstraintDecision(const double dz = desired.z() - pt.z(); if (verbose) { - ROS_INFO_NAMED( - "kinematic_constraints", "Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f", + RCLCPP_INFO(LOGGER_KINEMATIC_CONSTRAINTS, + "Position constraint %s on link '%s'. Desired: %f, %f, %f, current: %f, %f, %f", result ? "satisfied" : "violated", name.c_str(), desired.x(), desired.y(), desired.z(), pt.x(), pt.y(), pt.z()); - ROS_INFO_NAMED("kinematic_constraints", "Differences %g %g %g", dx, dy, dz); + RCLCPP_INFO(LOGGER_KINEMATIC_CONSTRAINTS, "Differences %g %g %g", dx, dy, dz); } return ConstraintEvaluationResult(result, weight * sqrt(dx * dx + dy * dy + dz * dz)); } @@ -498,21 +501,21 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra link_model_ = robot_model_->getLinkModel(oc.link_name); if (!link_model_) { - ROS_WARN_NAMED("kinematic_constraints", "Could not find link model for link name %s", oc.link_name.c_str()); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Could not find link model for link name %s", oc.link_name.c_str()); return false; } Eigen::Quaterniond q; tf2::fromMsg(oc.orientation, q); if (fabs(q.norm() - 1.0) > 1e-3) { - ROS_WARN_NAMED("kinematic_constraints", "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, " + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, " "%f. Assuming identity instead.", oc.link_name.c_str(), oc.orientation.x, oc.orientation.y, oc.orientation.z, oc.orientation.w); q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); } if (oc.header.frame_id.empty()) - ROS_WARN_NAMED("kinematic_constraints", "No frame specified for position constraint on link '%s'!", + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "No frame specified for position constraint on link '%s'!", oc.link_name.c_str()); if (tf.isFixedFrame(oc.header.frame_id)) @@ -531,12 +534,12 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra } std::stringstream matrix_str; matrix_str << desired_rotation_matrix_; - ROS_DEBUG_NAMED("kinematic_constraints", "The desired rotation matrix for link '%s' in frame %s is:\n%s", + RCLCPP_DEBUG(LOGGER_KINEMATIC_CONSTRAINTS, "The desired rotation matrix for link '%s' in frame %s is:\n%s", oc.link_name.c_str(), desired_rotation_frame_id_.c_str(), matrix_str.str().c_str()); if (oc.weight <= std::numeric_limits::epsilon()) { - ROS_WARN_NAMED("kinematic_constraints", + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "The weight on position constraint for link '%s' is near zero. Setting to 1.0.", oc.link_name.c_str()); constraint_weight_ = 1.0; @@ -545,13 +548,13 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra constraint_weight_ = oc.weight; absolute_x_axis_tolerance_ = fabs(oc.absolute_x_axis_tolerance); if (absolute_x_axis_tolerance_ < std::numeric_limits::epsilon()) - ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_x_axis_tolerance"); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Near-zero value for absolute_x_axis_tolerance"); absolute_y_axis_tolerance_ = fabs(oc.absolute_y_axis_tolerance); if (absolute_y_axis_tolerance_ < std::numeric_limits::epsilon()) - ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_y_axis_tolerance"); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Near-zero value for absolute_y_axis_tolerance"); absolute_z_axis_tolerance_ = fabs(oc.absolute_z_axis_tolerance); if (absolute_z_axis_tolerance_ < std::numeric_limits::epsilon()) - ROS_WARN_NAMED("kinematic_constraints", "Near-zero value for absolute_z_axis_tolerance"); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Near-zero value for absolute_z_axis_tolerance"); return link_model_ != nullptr; } @@ -620,7 +623,7 @@ ConstraintEvaluationResult OrientationConstraint::decide(const robot_state::Robo { Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).rotation()); Eigen::Quaterniond q_des(desired_rotation_matrix_); - ROS_INFO_NAMED("kinematic_constraints", + RCLCPP_INFO(LOGGER_KINEMATIC_CONSTRAINTS, "Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion " "actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f", result ? "satisfied" : "violated", link_model_->getName().c_str(), q_des.x(), q_des.y(), q_des.z(), @@ -671,12 +674,12 @@ bool VisibilityConstraint::configure(const moveit_msgs::msg::VisibilityConstrain target_radius_ = fabs(vc.target_radius); if (vc.target_radius <= std::numeric_limits::epsilon()) - ROS_WARN_NAMED("kinematic_constraints", + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "The radius of the target disc that must be visible should be strictly positive"); if (vc.cone_sides < 3) { - ROS_WARN_NAMED("kinematic_constraints", "The number of sides for the visibility region must be 3 or more. " + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "The number of sides for the visibility region must be 3 or more. " "Assuming 3 sides instead of the specified %d", vc.cone_sides); cone_sides_ = 3; @@ -728,7 +731,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::msg::VisibilityConstrain if (vc.weight <= std::numeric_limits::epsilon()) { - ROS_WARN_NAMED("kinematic_constraints", "The weight of visibility constraint is near zero. Setting to 1.0."); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "The weight of visibility constraint is near zero. Setting to 1.0."); constraint_weight_ = 1.0; } else @@ -848,17 +851,17 @@ shapes::Mesh* VisibilityConstraint::getVisibilityCone(const robot_state::RobotSt } void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, - visualization_msgs::MarkerArray& markers) const + visualization_msgs::msg::MarkerArray& markers) const { shapes::Mesh* m = getVisibilityCone(state); - visualization_msgs::Marker mk; + visualization_msgs::msg::Marker mk; shapes::constructMarkerFromShape(m, mk); delete m; mk.header.frame_id = robot_model_->getModelFrame(); - mk.header.stamp = ros::Time::now(); + mk.header.stamp = rclcpp::Clock().now(); mk.ns = "constraints"; mk.id = 1; - mk.action = visualization_msgs::Marker::ADD; + mk.action = visualization_msgs::msg::Marker::ADD; mk.pose.position.x = 0; mk.pose.position.y = 0; mk.pose.position.z = 0; @@ -866,7 +869,7 @@ void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, mk.pose.orientation.y = 0; mk.pose.orientation.z = 0; mk.pose.orientation.w = 1; - mk.lifetime = ros::Duration(60); + mk.lifetime = rclcpp::Duration(60); // this scale necessary to make results look reasonable mk.scale.x = .01; mk.color.a = 1.0; @@ -881,9 +884,9 @@ void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, const Eigen::Isometry3d& tp = mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; - visualization_msgs::Marker mka; - mka.type = visualization_msgs::Marker::ARROW; - mka.action = visualization_msgs::Marker::ADD; + visualization_msgs::msg::Marker mka; + mka.type = visualization_msgs::msg::Marker::ARROW; + mka.action = visualization_msgs::msg::Marker::ADD; mka.color = mk.color; mka.pose = mk.pose; @@ -942,14 +945,14 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot if (dp < 0.0) { if (verbose) - ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the sensor is looking at " + RCLCPP_INFO(LOGGER_KINEMATIC_CONSTRAINTS, "Visibility constraint is violated because the sensor is looking at " "the wrong side"); return ConstraintEvaluationResult(false, 0.0); } if (max_view_angle_ < ang) { if (verbose) - ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the view angle is %lf " + RCLCPP_INFO(LOGGER_KINEMATIC_CONSTRAINTS, "Visibility constraint is violated because the view angle is %lf " "(above the maximum allowed of %lf)", ang, max_view_angle_); return ConstraintEvaluationResult(false, 0.0); @@ -962,7 +965,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot if (dp < 0.0) { if (verbose) - ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the sensor is looking at " + RCLCPP_INFO(LOGGER_KINEMATIC_CONSTRAINTS, "Visibility constraint is violated because the sensor is looking at " "the wrong side"); return ConstraintEvaluationResult(false, 0.0); } @@ -971,7 +974,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot if (max_range_angle_ < ang) { if (verbose) - ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint is violated because the range angle is %lf " + RCLCPP_INFO(LOGGER_KINEMATIC_CONSTRAINTS, "Visibility constraint is violated because the range angle is %lf " "(above the maximum allowed of %lf)", ang, max_range_angle_); return ConstraintEvaluationResult(false, 0.0); @@ -991,7 +994,9 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot collision_detection::CollisionRequest req; collision_detection::CollisionResult res; collision_detection::AllowedCollisionMatrix acm; - acm.setDefaultEntry("cone", boost::bind(&VisibilityConstraint::decideContact, this, _1)); + collision_detection::DecideContactFn fn = std::bind(&VisibilityConstraint::decideContact, this, std::placeholders::_1); + acm.setDefaultEntry(std::string("cone"), fn); + req.contacts = true; req.verbose = verbose; req.max_contacts = 1; @@ -1001,7 +1006,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot { std::stringstream ss; m->print(ss); - ROS_INFO_NAMED("kinematic_constraints", "Visibility constraint %ssatisfied. Visibility cone approximation:\n %s", + RCLCPP_INFO(LOGGER_KINEMATIC_CONSTRAINTS, "Visibility constraint %ssatisfied. Visibility cone approximation:\n %s", res.collision ? "not " : "", ss.str().c_str()); } @@ -1018,7 +1023,7 @@ bool VisibilityConstraint::decideContact(const collision_detection::Contact& con (robot_state::Transforms::sameFrame(contact.body_name_1, sensor_frame_id_) || robot_state::Transforms::sameFrame(contact.body_name_1, target_frame_id_))) { - ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target"); + RCLCPP_DEBUG(LOGGER_KINEMATIC_CONSTRAINTS, "Accepted collision with either sensor or target"); return true; } if (contact.body_type_2 == collision_detection::BodyTypes::ROBOT_LINK && @@ -1026,7 +1031,7 @@ bool VisibilityConstraint::decideContact(const collision_detection::Contact& con (robot_state::Transforms::sameFrame(contact.body_name_2, sensor_frame_id_) || robot_state::Transforms::sameFrame(contact.body_name_2, target_frame_id_))) { - ROS_DEBUG_NAMED("kinematic_constraints", "Accepted collision with either sensor or target"); + RCLCPP_DEBUG(LOGGER_KINEMATIC_CONSTRAINTS, "Accepted collision with either sensor or target"); return true; } return false; diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 7df4ba9d06..4ab0621486 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -37,7 +37,6 @@ #include #include #include -#include using namespace moveit::core; @@ -45,6 +44,8 @@ namespace kinematic_constraints { const std::string LOGNAME = "kinematic_constraint_utils"; + rclcpp::Logger LOGGER_KINEMATIC_CONSTRAINTS_UTILS = rclcpp::get_logger(LOGNAME); + moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints& first, const moveit_msgs::msg::Constraints& second) { moveit_msgs::msg::Constraints r; @@ -64,10 +65,11 @@ moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constrain const moveit_msgs::msg::JointConstraint& b = second.joint_constraints[j]; double low = std::max(a.position - a.tolerance_below, b.position - b.tolerance_below); double high = std::min(a.position + a.tolerance_above, b.position + b.tolerance_above); - if (low > high) - ROS_ERROR_NAMED("kinematic_constraints", + if (low > high){ + RCLCPP_ERROR(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.", a.joint_name.c_str()); + } else { m.joint_name = a.joint_name; @@ -152,7 +154,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const robot_state::RobotS return goal; } -moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose, +moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::PoseStamped& pose, double tolerance_pos, double tolerance_angle) { moveit_msgs::msg::Constraints goal; @@ -164,10 +166,10 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n pcm.target_point_offset.y = 0; pcm.target_point_offset.z = 0; pcm.constraint_region.primitives.resize(1); - shape_msgs::SolidPrimitive& bv = pcm.constraint_region.primitives[0]; - bv.type = shape_msgs::SolidPrimitive::SPHERE; - bv.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); - bv.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance_pos; + shape_msgs::msg::SolidPrimitive& bv = pcm.constraint_region.primitives[0]; + bv.type = shape_msgs::msg::SolidPrimitive::SPHERE; + bv.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); + bv.dimensions[shape_msgs::msg::SolidPrimitive::SPHERE_RADIUS] = tolerance_pos; pcm.header = pose.header; pcm.constraint_region.primitive_poses.resize(1); @@ -193,19 +195,19 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n return goal; } -moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose, +moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::PoseStamped& pose, const std::vector& tolerance_pos, const std::vector& tolerance_angle) { moveit_msgs::msg::Constraints goal = constructGoalConstraints(link_name, pose); if (tolerance_pos.size() == 3) { - shape_msgs::SolidPrimitive& bv = goal.position_constraints[0].constraint_region.primitives[0]; - bv.type = shape_msgs::SolidPrimitive::BOX; - bv.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); - bv.dimensions[shape_msgs::SolidPrimitive::BOX_X] = tolerance_pos[0]; - bv.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = tolerance_pos[1]; - bv.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = tolerance_pos[2]; + shape_msgs::msg::SolidPrimitive& bv = goal.position_constraints[0].constraint_region.primitives[0]; + bv.type = shape_msgs::msg::SolidPrimitive::BOX; + bv.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount::value); + bv.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = tolerance_pos[0]; + bv.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = tolerance_pos[1]; + bv.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = tolerance_pos[2]; } if (tolerance_angle.size() == 3) { @@ -218,7 +220,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n } moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, - const geometry_msgs::QuaternionStamped& quat, double tolerance) + const geometry_msgs::msg::QuaternionStamped& quat, double tolerance) { moveit_msgs::msg::Constraints goal; goal.orientation_constraints.resize(1); @@ -234,9 +236,9 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n } moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, - const geometry_msgs::PointStamped& goal_point, double tolerance) + const geometry_msgs::msg::PointStamped& goal_point, double tolerance) { - geometry_msgs::Point p; + geometry_msgs::msg::Point p; p.x = 0; p.y = 0; p.z = 0; @@ -244,8 +246,8 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n } moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, - const geometry_msgs::Point& reference_point, - const geometry_msgs::PointStamped& goal_point, double tolerance) + const geometry_msgs::msg::Point& reference_point, + const geometry_msgs::msg::PointStamped& goal_point, double tolerance) { moveit_msgs::msg::Constraints goal; goal.position_constraints.resize(1); @@ -255,10 +257,10 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n pcm.target_point_offset.y = reference_point.y; pcm.target_point_offset.z = reference_point.z; 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( - geometric_shapes::SolidPrimitiveDimCount::value); - pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance; + geometric_shapes::SolidPrimitiveDimCount::value); + pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::SPHERE_RADIUS] = tolerance; pcm.header = goal_point.header; pcm.constraint_region.primitive_poses.resize(1); @@ -273,258 +275,260 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n return goal; } - -static bool constructPoseStamped(XmlRpc::XmlRpcValue::iterator& it, geometry_msgs::PoseStamped& pose) -{ - if (!isStruct(it->second, { "frame_id", "position", "orientation" }, it->first)) - return false; - pose.header.frame_id = static_cast(it->second["frame_id"]); - - if (!isArray(it->second["orientation"], 3, "orientation", "RPY values")) - return false; - auto& rpy = it->second["orientation"]; - tf2::Quaternion q; - q.setRPY(parseDouble(rpy[0]), parseDouble(rpy[1]), parseDouble(rpy[2])); - pose.pose.orientation = toMsg(q); - - if (!isArray(it->second["position"], 3, "position", "xyz position")) - return false; - pose.pose.position.x = parseDouble(it->second["position"][0]); - pose.pose.position.y = parseDouble(it->second["position"][1]); - pose.pose.position.z = parseDouble(it->second["position"][2]); - - return true; -} - -static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::JointConstraint& constraint) -{ - for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) - { - if (it->first == "type") - continue; - else if (it->first == "joint_name") - constraint.joint_name = static_cast(it->second); - else if (it->first == "weight") - constraint.weight = parseDouble(it->second); - else if (it->first == "position") - { - constraint.position = parseDouble(it->second); - } - else if (it->first == "tolerance") - { - constraint.tolerance_below = parseDouble(it->second); - constraint.tolerance_above = parseDouble(it->second); - } - else if (it->first == "tolerances") - { - if (!isArray(it->second, 2, it->first, "lower/upper tolerances")) - return false; - - constraint.tolerance_below = parseDouble(it->second[0]); - constraint.tolerance_above = parseDouble(it->second[1]); - } - else if (it->first == "bounds") - { - if (!isArray(it->second, 2, it->first, "lower/upper bound")) - return false; - - const double lower_bound = parseDouble(it->second[0]); - const double upper_bound = parseDouble(it->second[1]); - - constraint.position = (lower_bound + upper_bound) / 2; - constraint.tolerance_below = constraint.position - lower_bound; - constraint.tolerance_above = upper_bound - constraint.position; - } - else - { - ROS_WARN_STREAM_NAMED(LOGNAME, "joint constraint contains unknown entity '" << it->first << "'"); - } - } - return true; -} - -static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::PositionConstraint& constraint) -{ - for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) - { - if (it->first == "type") - continue; - else if (it->first == "frame_id") - constraint.header.frame_id = static_cast(it->second); - else if (it->first == "weight") - constraint.weight = parseDouble(it->second); - else if (it->first == "link_name") - constraint.link_name = static_cast(it->second); - else if (it->first == "target_offset") - { - if (!isArray(it->second, 3, it->first, "x/y/z position")) - return false; - - constraint.target_point_offset.x = parseDouble(it->second[0]); - constraint.target_point_offset.y = parseDouble(it->second[1]); - constraint.target_point_offset.z = parseDouble(it->second[2]); - } - else if (it->first == "region") - { - if (!isStruct(it->second, { "x", "y", "z" }, "region")) - return false; - - constraint.constraint_region.primitive_poses.emplace_back(); - constraint.constraint_region.primitives.emplace_back(); - - geometry_msgs::Pose& region_pose = constraint.constraint_region.primitive_poses.back(); - shape_msgs::SolidPrimitive& region_primitive = constraint.constraint_region.primitives.back(); - - region_primitive.type = shape_msgs::SolidPrimitive::BOX; - region_primitive.dimensions.resize(3); - - std::function parse_dimension = - [](XmlRpc::XmlRpcValue& it, double& center, double& dimension) { - center = (parseDouble(it[0]) + parseDouble(it[1])) / 2; - dimension = parseDouble(it[1]) - parseDouble(it[0]); - }; - - parse_dimension(it->second["x"], region_pose.position.x, - region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_X]); - parse_dimension(it->second["y"], region_pose.position.y, - region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Y]); - parse_dimension(it->second["z"], region_pose.position.z, - region_primitive.dimensions[shape_msgs::SolidPrimitive::BOX_Z]); - - region_pose.orientation.w = 1.0; - } - else - { - ROS_WARN_STREAM_NAMED(LOGNAME, "position constraint contains unknown entity '" << it->first << "'"); - } - } - return true; -} - -static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::OrientationConstraint& constraint) -{ - for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) - { - if (it->first == "type") - continue; - else if (it->first == "frame_id") - constraint.header.frame_id = static_cast(it->second); - else if (it->first == "weight") - constraint.weight = parseDouble(it->second); - else if (it->first == "link_name") - constraint.link_name = static_cast(it->second); - else if (it->first == "orientation") - { - if (!isArray(it->second, 3, it->first, "RPY values")) - return false; - - tf2::Quaternion q; - q.setRPY(parseDouble(it->second[0]), parseDouble(it->second[1]), parseDouble(it->second[2])); - constraint.orientation = toMsg(q); - } - else if (it->first == "tolerances") - { - if (!isArray(it->second, 3, it->first, "xyz tolerances")) - return false; - - constraint.absolute_x_axis_tolerance = parseDouble(it->second[0]); - constraint.absolute_y_axis_tolerance = parseDouble(it->second[1]); - constraint.absolute_z_axis_tolerance = parseDouble(it->second[2]); - } - else - { - ROS_WARN_STREAM_NAMED(LOGNAME, "orientation constraint contains unknown entity '" << it->first << "'"); - } - } - return true; -} - -static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::VisibilityConstraint& constraint) -{ - for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) - { - if (it->first == "type") - continue; - else if (it->first == "weight") - constraint.weight = parseDouble(it->second); - else if (it->first == "target_radius") - constraint.target_radius = parseDouble(it->second); - else if (it->first == "target_pose") - { - if (!constructPoseStamped(it, constraint.target_pose)) - return false; - } - else if (it->first == "cone_sides") - constraint.cone_sides = static_cast(it->second); - else if (it->first == "sensor_pose") - { - if (!constructPoseStamped(it, constraint.sensor_pose)) - return false; - } - else if (it->first == "max_view_angle") - constraint.max_view_angle = parseDouble(it->second); - else if (it->first == "max_range_angle") - constraint.max_range_angle = parseDouble(it->second); - else - { - ROS_WARN_STREAM_NAMED(LOGNAME, "orientation constraint contains unknown entity '" << it->first << "'"); - } - } - - constraint.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_X; - - return true; -} - -static bool collectConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints) -{ - if (params.getType() != XmlRpc::XmlRpcValue::TypeArray) - { - ROS_ERROR_NAMED(LOGNAME, "expected constraints as array"); - return false; - } - - for (int i = 0; i < params.size(); ++i) - { - if (!params[i].hasMember("type")) - { - ROS_ERROR_NAMED(LOGNAME, "constraint parameter does not specify its type"); - } - else if (params[i]["type"] == "joint") - { - constraints.joint_constraints.emplace_back(); - if (!constructConstraint(params[i], constraints.joint_constraints.back())) - return false; - } - else if (params[i]["type"] == "position") - { - constraints.position_constraints.emplace_back(); - if (!constructConstraint(params[i], constraints.position_constraints.back())) - return false; - } - else if (params[i]["type"] == "orientation") - { - constraints.orientation_constraints.emplace_back(); - if (!constructConstraint(params[i], constraints.orientation_constraints.back())) - return false; - } - else if (params[i]["type"] == "visibility") - { - constraints.visibility_constraints.emplace_back(); - if (!constructConstraint(params[i], constraints.visibility_constraints.back())) - return false; - } - } - - return true; -} - -bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints) -{ - if (!isStruct(params, { "name", "constraints" }, "Parameter")) - return false; - - constraints.name = static_cast(params["name"]); - return collectConstraints(params["constraints"], constraints); +//TODO ROS 2: Rework these functions + +// TODO: Is not getting called from anywhere, check if is necessary +// static bool constructPoseStamped(XmlRpc::XmlRpcValue::iterator& it, geometry_msgs::msg::PoseStamped& pose) +// { +// if (!isStruct(it->second, { "frame_id", "position", "orientation" }, it->first)) +// return false; +// pose.header.frame_id = static_cast(it->second["frame_id"]); +// +// if (!isArray(it->second["orientation"], 3, "orientation", "RPY values")) +// return false; +// auto& rpy = it->second["orientation"]; +// tf2::Quaternion q; +// q.setRPY(parseDouble(rpy[0]), parseDouble(rpy[1]), parseDouble(rpy[2])); +// pose.pose.orientation = toMsg(q); +// +// if (!isArray(it->second["position"], 3, "position", "xyz position")) +// return false; +// pose.pose.position.x = parseDouble(it->second["position"][0]); +// pose.pose.position.y = parseDouble(it->second["position"][1]); +// pose.pose.position.z = parseDouble(it->second["position"][2]); +// +// return true; +// } +// +// static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::JointConstraint& constraint) +// { +// for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) +// { +// if (it->first == "type") +// continue; +// else if (it->first == "joint_name") +// constraint.joint_name = static_cast(it->second); +// else if (it->first == "weight") +// constraint.weight = parseDouble(it->second); +// else if (it->first == "position") +// { +// constraint.position = parseDouble(it->second); +// } +// else if (it->first == "tolerance") +// { +// constraint.tolerance_below = parseDouble(it->second); +// constraint.tolerance_above = parseDouble(it->second); +// } +// else if (it->first == "tolerances") +// { +// if (!isArray(it->second, 2, it->first, "lower/upper tolerances")) +// return false; +// +// constraint.tolerance_below = parseDouble(it->second[0]); +// constraint.tolerance_above = parseDouble(it->second[1]); +// } +// else if (it->first == "bounds") +// { +// if (!isArray(it->second, 2, it->first, "lower/upper bound")) +// return false; +// +// const double lower_bound = parseDouble(it->second[0]); +// const double upper_bound = parseDouble(it->second[1]); +// +// constraint.position = (lower_bound + upper_bound) / 2; +// constraint.tolerance_below = constraint.position - lower_bound; +// constraint.tolerance_above = upper_bound - constraint.position; +// } +// else +// { +// RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "joint constraint contains unknown entity '" << it->first << "'"); +// } +// } +// return true; +// } +// +// static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::PositionConstraint& constraint) +// { +// for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) +// { +// if (it->first == "type") +// continue; +// else if (it->first == "frame_id") +// constraint.header.frame_id = static_cast(it->second); +// else if (it->first == "weight") +// constraint.weight = parseDouble(it->second); +// else if (it->first == "link_name") +// constraint.link_name = static_cast(it->second); +// else if (it->first == "target_offset") +// { +// if (!isArray(it->second, 3, it->first, "x/y/z position")) +// return false; +// +// constraint.target_point_offset.x = parseDouble(it->second[0]); +// constraint.target_point_offset.y = parseDouble(it->second[1]); +// constraint.target_point_offset.z = parseDouble(it->second[2]); +// } +// else if (it->first == "region") +// { +// if (!isStruct(it->second, { "x", "y", "z" }, "region")) +// return false; +// +// constraint.constraint_region.primitive_poses.emplace_back(); +// constraint.constraint_region.primitives.emplace_back(); +// +// geometry_msgs::msg::Pose& region_pose = constraint.constraint_region.primitive_poses.back(); +// shape_msgs::msg::SolidPrimitive& region_primitive = constraint.constraint_region.primitives.back(); +// +// region_primitive.type = shape_msgs::msg::SolidPrimitive::BOX; +// region_primitive.dimensions.resize(3); +// +// std::function parse_dimension = +// [](XmlRpc::XmlRpcValue& it, double& center, double& dimension) { +// center = (parseDouble(it[0]) + parseDouble(it[1])) / 2; +// dimension = parseDouble(it[1]) - parseDouble(it[0]); +// }; +// +// parse_dimension(it->second["x"], region_pose.position.x, +// region_primitive.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X]); +// parse_dimension(it->second["y"], region_pose.position.y, +// region_primitive.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y]); +// parse_dimension(it->second["z"], region_pose.position.z, +// region_primitive.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z]); +// +// region_pose.orientation.w = 1.0; +// } +// else +// { +// RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "position constraint contains unknown entity '" << it->first << "'"); +// } +// } +// return true; +// } +// +// static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::OrientationConstraint& constraint) +// { +// for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) +// { +// if (it->first == "type") +// continue; +// else if (it->first == "frame_id") +// constraint.header.frame_id = static_cast(it->second); +// else if (it->first == "weight") +// constraint.weight = parseDouble(it->second); +// else if (it->first == "link_name") +// constraint.link_name = static_cast(it->second); +// else if (it->first == "orientation") +// { +// if (!isArray(it->second, 3, it->first, "RPY values")) +// return false; +// +// tf2::Quaternion q; +// q.setRPY(parseDouble(it->second[0]), parseDouble(it->second[1]), parseDouble(it->second[2])); +// constraint.orientation = toMsg(q); +// } +// else if (it->first == "tolerances") +// { +// if (!isArray(it->second, 3, it->first, "xyz tolerances")) +// return false; +// +// constraint.absolute_x_axis_tolerance = parseDouble(it->second[0]); +// constraint.absolute_y_axis_tolerance = parseDouble(it->second[1]); +// constraint.absolute_z_axis_tolerance = parseDouble(it->second[2]); +// } +// else +// { +// RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "orientation constraint contains unknown entity '" << it->first << "'"); +// } +// } +// return true; +// } +// +// static bool constructConstraint(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::VisibilityConstraint& constraint) +// { +// for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) +// { +// if (it->first == "type") +// continue; +// else if (it->first == "weight") +// constraint.weight = parseDouble(it->second); +// else if (it->first == "target_radius") +// constraint.target_radius = parseDouble(it->second); +// else if (it->first == "target_pose") +// { +// if (!constructPoseStamped(it, constraint.target_pose)) +// return false; +// } +// else if (it->first == "cone_sides") +// constraint.cone_sides = static_cast(it->second); +// else if (it->first == "sensor_pose") +// { +// if (!constructPoseStamped(it, constraint.sensor_pose)) +// return false; +// } +// else if (it->first == "max_view_angle") +// constraint.max_view_angle = parseDouble(it->second); +// else if (it->first == "max_range_angle") +// constraint.max_range_angle = parseDouble(it->second); +// else +// { +// RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "orientation constraint contains unknown entity '" << it->first << "'"); +// } +// } +// +// constraint.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_X; +// +// return true; +// } +// +// static bool collectConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints) +// { +// if (params.getType() != XmlRpc::XmlRpcValue::TypeArray) +// { +// RCLCPP_ERROR(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "expected constraints as array"); +// return false; +// } +// +// for (int i = 0; i < params.size(); ++i) +// { +// if (!params[i].hasMember("type")) +// { +// RCLCPP_ERROR(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "constraint parameter does not specify its type"); +// } +// else if (params[i]["type"] == "joint") +// { +// constraints.joint_constraints.emplace_back(); +// if (!constructConstraint(params[i], constraints.joint_constraints.back())) +// return false; +// } +// else if (params[i]["type"] == "position") +// { +// constraints.position_constraints.emplace_back(); +// if (!constructConstraint(params[i], constraints.position_constraints.back())) +// return false; +// } +// else if (params[i]["type"] == "orientation") +// { +// constraints.orientation_constraints.emplace_back(); +// if (!constructConstraint(params[i], constraints.orientation_constraints.back())) +// return false; +// } +// else if (params[i]["type"] == "visibility") +// { +// constraints.visibility_constraints.emplace_back(); +// if (!constructConstraint(params[i], constraints.visibility_constraints.back())) +// return false; +// } +// } +// +// return true; +// } +// +// bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints) +// { +// if (!isStruct(params, { "name", "constraints" }, "Parameter")) +// return false; +// +// constraints.name = static_cast(params["name"]); +// return collectConstraints(params["constraints"], constraints); +// } } -} // namespace kinematic_constraints diff --git a/moveit_core/kinematic_constraints/test/test_constraints.cpp b/moveit_core/kinematic_constraints/test/test_constraints.cpp index c25daf5455..4f493c8dd9 100644 --- a/moveit_core/kinematic_constraints/test/test_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_constraints.cpp @@ -371,7 +371,7 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsFixed) 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; // no dimensions, so no valid regions EXPECT_FALSE(pc.configure(pcm, tf)); @@ -446,9 +446,9 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsMobile) 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[shape_msgs::SolidPrimitive::BOX_X] = 0.38 * 2.0; + pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 0.38 * 2.0; pcm.header.frame_id = "r_wrist_roll_link"; @@ -468,11 +468,11 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsMobile) EXPECT_TRUE(pc.decide(robot_state).satisfied); - pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::BOX; + pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; pcm.constraint_region.primitives[0].dimensions.resize(3); - pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.1; - pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1; - pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.1; + pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 0.1; + pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 0.1; + pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = 0.1; EXPECT_TRUE(pc.configure(pcm, tf)); std::map jvals; @@ -498,11 +498,11 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsMobile) pcm.constraint_region.primitive_poses[1].orientation.w = 1.0; pcm.constraint_region.primitives.resize(2); - pcm.constraint_region.primitives[1].type = shape_msgs::SolidPrimitive::BOX; + pcm.constraint_region.primitives[1].type = shape_msgs::msg::SolidPrimitive::BOX; pcm.constraint_region.primitives[1].dimensions.resize(3); - pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.1; - pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1; - pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.1; + pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 0.1; + pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 0.1; + pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = 0.1; EXPECT_TRUE(pc.configure(pcm, tf)); EXPECT_TRUE(pc.decide(robot_state, false).satisfied); } @@ -523,14 +523,14 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsEquality) pcm.target_point_offset.z = 0; pcm.constraint_region.primitives.resize(2); - 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.38 * 2.0; - pcm.constraint_region.primitives[1].type = shape_msgs::SolidPrimitive::BOX; + pcm.constraint_region.primitives[1].type = shape_msgs::msg::SolidPrimitive::BOX; pcm.constraint_region.primitives[1].dimensions.resize(3); - pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 2.0; - pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 2.0; - pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 2.0; + pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = 2.0; + pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = 2.0; + pcm.constraint_region.primitives[1].dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = 2.0; pcm.header.frame_id = "r_wrist_roll_link"; pcm.constraint_region.primitive_poses.resize(2); @@ -588,7 +588,7 @@ TEST_F(LoadPlanningModelsPr2, PositionConstraintsEquality) // changing the shape also changes it pcm2.constraint_region.primitives[2].dimensions[0] = pcm2.constraint_region.primitives[0].dimensions[0]; - pcm2.constraint_region.primitives[2].type = shape_msgs::SolidPrimitive::SPHERE; + pcm2.constraint_region.primitives[2].type = shape_msgs::msg::SolidPrimitive::SPHERE; EXPECT_TRUE(pc2.configure(pcm2, tf)); EXPECT_FALSE(pc.equal(pc2, .001)); } @@ -635,7 +635,7 @@ TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple) ASSERT_TRUE(oc.getLinkModel()); - geometry_msgs::Pose p = tf2::toMsg(robot_state.getGlobalLinkTransform(oc.getLinkModel()->getName())); + geometry_msgs::msg::Pose p = tf2::toMsg(robot_state.getGlobalLinkTransform(oc.getLinkModel()->getName())); ocm.orientation = p.orientation; ocm.header.frame_id = robot_model_->getModelFrame(); @@ -867,7 +867,7 @@ TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSetEquality) 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.2;