From 7cdb75698e9bd7046b288b884f114c0286ecc8c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 10 Mar 2019 18:12:36 +0100 Subject: [PATCH 01/12] Port kinematic_constraints to ROS2 Authored by @anasarrak --- .../kinematic_constraints/CMakeLists.txt | 37 +- .../kinematic_constraint.h | 22 +- .../moveit/kinematic_constraints/utils.h | 22 +- .../src/kinematic_constraint.cpp | 99 ++-- .../kinematic_constraints/src/utils.cpp | 557 +++++++++--------- 5 files changed, 386 insertions(+), 351 deletions(-) diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index ea45aa72f0..2b4f07e667 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -5,21 +5,30 @@ add_library(${MOVEIT_LIB_NAME} src/utils.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -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}) +ament_target_dependencies(${MOVEIT_LIB_NAME} + urdf + urdfdom + urdfdom_headers + moveit_robot_model + tf2_geometry_msgs + visualization_msgs + moveit_kinematics_base + moveit_robot_state + tf2_eigen + moveit_collision_detection_fcl) 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) - find_package(moveit_resources 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}) -endif() +#TODO: Review once moveit_resources has been ported +# if(BUILD_TESTING) +# find_package(ament_cmake_gtest REQUIRED) +# find_package(moveit_resources REQUIRED) +# include_directories(${moveit_resources_INCLUDE_DIRS}) +# +# ament_add_gtest(test_constraints test/test_constraints.cpp) +# target_link_libraries(test_constraints moveit_test_utils ${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 f6d8e9b1c2..722685ff74 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 @@ -44,11 +44,15 @@ #include #include -#include +#include #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,20 @@ 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; + + /** + * \brief Adds markers associated with the visibility cone, sensor + * and target to the visualization array + * + * The visibility cone and two arrows - a blue array that issues + * from the sensor_view_direction of the sensor, and a red arrow the + * issues along the Z axis of the the target frame. + * + * @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::msg::MarkerArray& markers); bool enabled() const override; ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override; @@ -846,6 +863,7 @@ class VisibilityConstraint : public KinematicConstraint double target_radius_; /**< \brief Storage for the target radius */ double max_view_angle_; /**< \brief Storage for the max view angle */ double max_range_angle_; /**< \brief Storage for the max range angle */ + rclcpp::Clock clock_ros_; /**< \brief ros2 clock for the time */ }; MOVEIT_CLASS_FORWARD(KinematicConstraintSet); 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 615b97d800..4c2fed7993 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -37,10 +37,10 @@ #ifndef MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_ #define MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_ -#include -#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,7 @@ 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); +// 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..80e65678e4 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,18 @@ 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) { shapes::Mesh* m = getVisibilityCone(state); - visualization_msgs::Marker mk; + visualization_msgs::msg::Marker mk; shapes::constructMarkerFromShape(m, mk); + rclcpp::Time stamp = clock_ros_.now(); delete m; mk.header.frame_id = robot_model_->getModelFrame(); - mk.header.stamp = ros::Time::now(); + mk.header.stamp = stamp; 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 +870,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 +885,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 +946,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 +966,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 +975,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 +995,8 @@ 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)); + acm.setDefaultEntry("cone", (boost::function) + boost::bind(&VisibilityConstraint::decideContact, this, _1)); 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 77c3295bf5..5b472fdbab 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +//#include using namespace moveit::core; @@ -45,6 +45,8 @@ namespace kinematic_constraints { const std::string LOGNAME = "kinematic_constraint_utils"; + rclcpp::Logger logger_kinematic_constraints = 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 +66,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, "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.", a.joint_name.c_str()); + } else { m.joint_name = a.joint_name; @@ -152,7 +155,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 +167,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 +196,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 +221,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 +237,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 +247,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 +258,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 +276,258 @@ 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: 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, "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, "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, "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, "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, "expected constraints as array"); +// return false; +// } +// +// for (int i = 0; i < params.size(); ++i) +// { +// if (!params[i].hasMember("type")) +// { +// RCLCPP_ERROR(logger_kinematic_constraints, "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); +// } } From 5ace7e8c2d820dca28d1b95a52c566d039a901bd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sat, 30 Mar 2019 22:06:49 +0100 Subject: [PATCH 02/12] kinematic-constraints, first steps towards unit tests Still not building but fixes ROS 2 APIs --- .../kinematic_constraints/CMakeLists.txt | 10 ++--- .../test/test_constraints.cpp | 38 +++++++++---------- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index 2b4f07e667..04f7684bc8 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -4,7 +4,6 @@ add_library(${MOVEIT_LIB_NAME} src/kinematic_constraint.cpp src/utils.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - ament_target_dependencies(${MOVEIT_LIB_NAME} urdf urdfdom @@ -20,15 +19,16 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) - install(DIRECTORY include/ DESTINATION include) -#TODO: Review once moveit_resources has been ported # if(BUILD_TESTING) # find_package(ament_cmake_gtest REQUIRED) # find_package(moveit_resources REQUIRED) # include_directories(${moveit_resources_INCLUDE_DIRS}) -# # ament_add_gtest(test_constraints test/test_constraints.cpp) -# target_link_libraries(test_constraints moveit_test_utils ${MOVEIT_LIB_NAME}) +# target_link_libraries(test_constraints +# moveit_test_utils +# moveit_collision_detection +# moveit_collision_detection_fcl +# ${MOVEIT_LIB_NAME}) # endif() 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; From 8d49463ba1e1e2b72c2b0cd7fae54d6d0437cc32 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 26 Apr 2019 10:07:37 +0200 Subject: [PATCH 03/12] Fixing kinematic constraints tests --- .../kinematic_constraints/CMakeLists.txt | 51 ++++++++++++++----- .../kinematic_constraints/src/utils.cpp | 16 +++--- 2 files changed, 46 insertions(+), 21 deletions(-) diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index 04f7684bc8..9137a25bad 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -1,9 +1,17 @@ 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) + set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + ament_target_dependencies(${MOVEIT_LIB_NAME} urdf urdfdom @@ -14,21 +22,38 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} moveit_kinematics_base moveit_robot_state tf2_eigen - moveit_collision_detection_fcl) +) + +target_link_libraries(${MOVEIT_LIB_NAME} + moveit_collision_detection_fcl +) + install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) + install(DIRECTORY include/ DESTINATION include) -# if(BUILD_TESTING) -# find_package(ament_cmake_gtest REQUIRED) -# find_package(moveit_resources REQUIRED) -# include_directories(${moveit_resources_INCLUDE_DIRS}) -# ament_add_gtest(test_constraints test/test_constraints.cpp) -# target_link_libraries(test_constraints -# moveit_test_utils -# moveit_collision_detection -# moveit_collision_detection_fcl -# ${MOVEIT_LIB_NAME}) -# endif() +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}) + + 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/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 5b472fdbab..3e6045782b 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -45,7 +45,7 @@ namespace kinematic_constraints { const std::string LOGNAME = "kinematic_constraint_utils"; - rclcpp::Logger logger_kinematic_constraints = rclcpp::get_logger(LOGNAME); + 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) { @@ -67,7 +67,7 @@ moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constrain 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){ - RCLCPP_ERROR(logger_kinematic_constraints, + RCLCPP_ERROR(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.", a.joint_name.c_str()); } @@ -340,7 +340,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n // } // else // { -// RCLCPP_WARN(logger_kinematic_constraints, "joint constraint contains unknown entity '" << it->first << "'"); +// RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "joint constraint contains unknown entity '" << it->first << "'"); // } // } // return true; @@ -398,7 +398,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n // } // else // { -// RCLCPP_WARN(logger_kinematic_constraints, "position constraint contains unknown entity '" << it->first << "'"); +// RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "position constraint contains unknown entity '" << it->first << "'"); // } // } // return true; @@ -436,7 +436,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n // } // else // { -// RCLCPP_WARN(logger_kinematic_constraints, "orientation constraint contains unknown entity '" << it->first << "'"); +// RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "orientation constraint contains unknown entity '" << it->first << "'"); // } // } // return true; @@ -470,7 +470,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n // constraint.max_range_angle = parseDouble(it->second); // else // { -// RCLCPP_WARN(logger_kinematic_constraints, "orientation constraint contains unknown entity '" << it->first << "'"); +// RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "orientation constraint contains unknown entity '" << it->first << "'"); // } // } // @@ -483,7 +483,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n // { // if (params.getType() != XmlRpc::XmlRpcValue::TypeArray) // { -// RCLCPP_ERROR(logger_kinematic_constraints, "expected constraints as array"); +// RCLCPP_ERROR(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "expected constraints as array"); // return false; // } // @@ -491,7 +491,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n // { // if (!params[i].hasMember("type")) // { -// RCLCPP_ERROR(logger_kinematic_constraints, "constraint parameter does not specify its type"); +// RCLCPP_ERROR(LOGGER_KINEMATIC_CONSTRAINTS_UTILS, "constraint parameter does not specify its type"); // } // else if (params[i]["type"] == "joint") // { From d70f8888f716d3773e92c213af01d562f8883ab5 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 8 May 2019 15:46:23 +0200 Subject: [PATCH 04/12] kinematic_constraints adding library dependencies --- moveit_core/kinematic_constraints/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index 9137a25bad..0f26c779dc 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -16,16 +16,16 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} urdf urdfdom urdfdom_headers - moveit_robot_model tf2_geometry_msgs visualization_msgs - moveit_kinematics_base - moveit_robot_state tf2_eigen ) target_link_libraries(${MOVEIT_LIB_NAME} moveit_collision_detection_fcl + moveit_kinematics_base + moveit_robot_state + moveit_robot_model ) From beb3bcaa1ca4de4d175018102e7014dbe9bb9593 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 24 May 2019 13:20:55 +0200 Subject: [PATCH 05/12] moveit_core kinematics_constraints - addressing some issues --- .../include/moveit/kinematic_constraints/utils.h | 1 + .../kinematic_constraints/src/kinematic_constraint.cpp | 5 ++--- moveit_core/kinematic_constraints/src/utils.cpp | 3 ++- 3 files changed, 5 insertions(+), 4 deletions(-) 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 4c2fed7993..490dde72e1 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h @@ -200,6 +200,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n * * @return was the construction successful? */ + // TODO rework this function // bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints); } diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 80e65678e4..e0dc4f8f42 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -851,15 +851,14 @@ shapes::Mesh* VisibilityConstraint::getVisibilityCone(const robot_state::RobotSt } void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, - visualization_msgs::msg::MarkerArray& markers) + visualization_msgs::msg::MarkerArray& markers) const { shapes::Mesh* m = getVisibilityCone(state); visualization_msgs::msg::Marker mk; shapes::constructMarkerFromShape(m, mk); - rclcpp::Time stamp = clock_ros_.now(); delete m; mk.header.frame_id = robot_model_->getModelFrame(); - mk.header.stamp = stamp; + mk.header.stamp = rclcpp::Clock().now(); mk.ns = "constraints"; mk.id = 1; mk.action = visualization_msgs::msg::Marker::ADD; diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 3e6045782b..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; @@ -276,6 +275,8 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n return goal; } +//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) // { From 6b1dd61d45e573852930b055a6a5cded19cd31a2 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 29 May 2019 23:32:38 +0200 Subject: [PATCH 06/12] moveit_core kinematics_constraints - logger_kinematic_constraints to LOGGER_KINEMATIC_CONSTRAINTS --- .../src/kinematic_constraint.cpp | 74 +++++++++---------- 1 file changed, 37 insertions(+), 37 deletions(-) diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index e0dc4f8f42..151a548bb8 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -49,7 +49,7 @@ namespace kinematic_constraints { -rclcpp::Logger logger_kinematic_constraints = rclcpp::get_logger("kinematic_constraints"); +rclcpp::Logger LOGGER_KINEMATIC_CONSTRAINTS = rclcpp::get_logger("kinematic_constraints"); static double normalizeAngle(double angle) { @@ -76,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) { - RCLCPP_WARN(logger_kinematic_constraints, "JointConstraint tolerance values must be positive."); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "JointConstraint tolerance values must be positive."); joint_model_ = nullptr; return false; } @@ -105,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) { - RCLCPP_ERROR(logger_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) { - RCLCPP_ERROR(logger_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; @@ -128,7 +128,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) } if (found < 0) { - RCLCPP_ERROR(logger_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; } @@ -169,7 +169,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) { joint_position_ = bounds.min_position_; joint_tolerance_above_ = std::numeric_limits::epsilon(); - RCLCPP_WARN(logger_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()); } @@ -177,7 +177,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) { joint_position_ = bounds.max_position_; joint_tolerance_below_ = std::numeric_limits::epsilon(); - RCLCPP_WARN(logger_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()); } @@ -185,7 +185,7 @@ bool JointConstraint::configure(const moveit_msgs::msg::JointConstraint& jc) if (jc.weight <= std::numeric_limits::epsilon()) { - RCLCPP_WARN(logger_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; @@ -233,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) - RCLCPP_INFO(logger_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_); @@ -280,7 +280,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p link_model_ = robot_model_->getLinkModel(pc.link_name); if (link_model_ == nullptr) { - RCLCPP_WARN(logger_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; @@ -288,7 +288,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p if (pc.header.frame_id.empty()) { - RCLCPP_WARN(logger_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; } @@ -315,7 +315,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p { if (pc.constraint_region.primitive_poses.size() <= i) { - RCLCPP_WARN(logger_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()))); @@ -331,7 +331,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p } } else - RCLCPP_WARN(logger_kinematic_constraints, "Could not construct primitive shape %zu", i); + RCLCPP_WARN(LOGGER_KINEMATIC_CONSTRAINTS, "Could not construct primitive shape %zu", i); } // load meshes @@ -342,7 +342,7 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p { if (pc.constraint_region.mesh_poses.size() <= i) { - RCLCPP_WARN(logger_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()))); @@ -359,13 +359,13 @@ bool PositionConstraint::configure(const moveit_msgs::msg::PositionConstraint& p } else { - RCLCPP_WARN(logger_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()) { - RCLCPP_WARN(logger_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; @@ -425,10 +425,10 @@ static inline ConstraintEvaluationResult finishPositionConstraintDecision(const double dz = desired.z() - pt.z(); if (verbose) { - RCLCPP_INFO(logger_kinematic_constraints, + 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()); - RCLCPP_INFO(logger_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)); } @@ -501,21 +501,21 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra link_model_ = robot_model_->getLinkModel(oc.link_name); if (!link_model_) { - RCLCPP_WARN(logger_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) { - RCLCPP_WARN(logger_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()) - RCLCPP_WARN(logger_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)) @@ -534,12 +534,12 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra } std::stringstream matrix_str; matrix_str << desired_rotation_matrix_; - RCLCPP_DEBUG(logger_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()) { - RCLCPP_WARN(logger_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; @@ -548,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()) - RCLCPP_WARN(logger_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()) - RCLCPP_WARN(logger_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()) - RCLCPP_WARN(logger_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; } @@ -623,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_); - RCLCPP_INFO(logger_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(), @@ -674,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()) - RCLCPP_WARN(logger_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) { - RCLCPP_WARN(logger_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; @@ -731,7 +731,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::msg::VisibilityConstrain if (vc.weight <= std::numeric_limits::epsilon()) { - RCLCPP_WARN(logger_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 @@ -945,14 +945,14 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot if (dp < 0.0) { if (verbose) - RCLCPP_INFO(logger_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) - RCLCPP_INFO(logger_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); @@ -965,7 +965,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot if (dp < 0.0) { if (verbose) - RCLCPP_INFO(logger_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); } @@ -974,7 +974,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot if (max_range_angle_ < ang) { if (verbose) - RCLCPP_INFO(logger_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); @@ -1005,7 +1005,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot { std::stringstream ss; m->print(ss); - RCLCPP_INFO(logger_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()); } @@ -1022,7 +1022,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_))) { - RCLCPP_DEBUG(logger_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 && @@ -1030,7 +1030,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_))) { - RCLCPP_DEBUG(logger_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; From 57f271ee3bb61cf401da31608f3ea49a46409473 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 29 May 2019 23:36:02 +0200 Subject: [PATCH 07/12] moveit_core kinematics_constraints - remove duplicated getMarkers --- .../kinematic_constraints/kinematic_constraint.h | 13 ------------- 1 file changed, 13 deletions(-) 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 722685ff74..32cae33afd 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 @@ -820,19 +820,6 @@ class VisibilityConstraint : public KinematicConstraint */ void getMarkers(const robot_state::RobotState& state, visualization_msgs::msg::MarkerArray& markers) const; - /** - * \brief Adds markers associated with the visibility cone, sensor - * and target to the visualization array - * - * The visibility cone and two arrows - a blue array that issues - * from the sensor_view_direction of the sensor, and a red arrow the - * issues along the Z axis of the the target frame. - * - * @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::msg::MarkerArray& markers); - bool enabled() const override; ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override; void print(std::ostream& out = std::cout) const override; From c6aa8e666ed7bb2f7a10dd4d870bac8df9e03c72 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 29 May 2019 23:45:39 +0200 Subject: [PATCH 08/12] moveit_core kinematics_constraints - fix whitespaces in CMakeLists.txt --- .../kinematic_constraints/CMakeLists.txt | 49 ++++++++++--------- 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index 0f26c779dc..f976a189d0 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -1,14 +1,15 @@ set(MOVEIT_LIB_NAME moveit_kinematic_constraints) if(WIN32) - # set(append_library_dirs "$;$") +# 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") +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}") @@ -22,38 +23,38 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} ) target_link_libraries(${MOVEIT_LIB_NAME} - moveit_collision_detection_fcl - moveit_kinematics_base - moveit_robot_state - moveit_robot_model + moveit_collision_detection_fcl + moveit_kinematics_base + moveit_robot_state + moveit_robot_model ) - install(TARGETS ${MOVEIT_LIB_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib) + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) install(DIRECTORY include/ DESTINATION include) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(moveit_resources REQUIRED) - find_package(resource_retriever REQUIRED) + find_package(resource_retriever REQUIRED) include_directories(${moveit_resources_INCLUDE_DIRS}) - 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 + 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() From df09d3a9b228cf0f969031762cfd3554476f019a Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 29 May 2019 23:46:14 +0200 Subject: [PATCH 09/12] moveit_core kinematics_constraints - fix whitespaces in CMakeLists.txt --- moveit_core/kinematic_constraints/CMakeLists.txt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index f976a189d0..d7fa3f2c05 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -1,9 +1,9 @@ set(MOVEIT_LIB_NAME moveit_kinematic_constraints) if(WIN32) -# set(append_library_dirs "$;$") + # 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") + 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 @@ -44,7 +44,8 @@ if(BUILD_TESTING) include_directories(${moveit_resources_INCLUDE_DIRS}) ament_add_gtest(test_constraints test/test_constraints.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") + APPEND_LIBRARY_DIRS "${append_library_dirs}" + ) target_link_libraries(test_constraints moveit_kinematic_constraints From da78630b9a4c960a31041bb8c2baf236260b0a38 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 30 May 2019 12:18:44 +0200 Subject: [PATCH 10/12] moveit_core kinematic_constraints - Fix getMarkers header --- .../include/moveit/kinematic_constraints/kinematic_constraint.h | 1 - moveit_core/kinematic_constraints/src/kinematic_constraint.cpp | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) 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 32cae33afd..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 @@ -850,7 +850,6 @@ class VisibilityConstraint : public KinematicConstraint double target_radius_; /**< \brief Storage for the target radius */ double max_view_angle_; /**< \brief Storage for the max view angle */ double max_range_angle_; /**< \brief Storage for the max range angle */ - rclcpp::Clock clock_ros_; /**< \brief ros2 clock for the time */ }; MOVEIT_CLASS_FORWARD(KinematicConstraintSet); diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 151a548bb8..da9d0c2f64 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -856,6 +856,7 @@ void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, shapes::Mesh* m = getVisibilityCone(state); visualization_msgs::msg::Marker mk; shapes::constructMarkerFromShape(m, mk); + rclcpp::Time stamp = rclcpp::Clock().now(); delete m; mk.header.frame_id = robot_model_->getModelFrame(); mk.header.stamp = rclcpp::Clock().now(); From b3855c3fb738fad92a5a8f4ae74c5068c23620aa Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 4 Jun 2019 17:19:03 +0200 Subject: [PATCH 11/12] moveit_core kinematics constraints - converting boost to std --- .../collision_detection/CMakeLists.txt | 47 +++++++++++++++---- .../collision_detection/collision_matrix.h | 3 +- .../src/kinematic_constraint.cpp | 5 +- 3 files changed, 43 insertions(+), 12 deletions(-) diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt index bfc4e8db6b..f805244fb7 100644 --- a/moveit_core/collision_detection/CMakeLists.txt +++ b/moveit_core/collision_detection/CMakeLists.txt @@ -11,24 +11,55 @@ add_library(${MOVEIT_LIB_NAME} src/world.cpp src/world_diff.cpp ) -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +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 +) target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_state ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) # unit tests if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(test_world test/test_world.cpp) - target_link_libraries(test_world ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) + ament_add_gtest(test_world test/test_world.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + target_link_libraries(test_world + ${MOVEIT_LIB_NAME} + ${urdfdom} + ${urdfdom_headers} + ${geometric_shapes_LIBRARIES} + ) - catkin_add_gtest(test_world_diff test/test_world_diff.cpp) - target_link_libraries(test_world_diff ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) + ament_add_gtest(test_world_diff test/test_world_diff.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + target_link_libraries(test_world_diff + ${MOVEIT_LIB_NAME} + ${urdfdom} + ${urdfdom_headers} + ${geometric_shapes_LIBRARIES} + ) - catkin_add_gtest(test_all_valid test/test_all_valid.cpp) - target_link_libraries(test_all_valid ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) + ament_add_gtest(test_all_valid test/test_all_valid.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") + target_link_libraries(test_all_valid + ${MOVEIT_LIB_NAME} + moveit_robot_model + ${urdfdom} + ${urdfdom_headers} + ${geometric_shapes_LIBRARIES} + resource_retriever::resource_retriever + ) endif() - install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 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 d8e3476042..df745d78ac 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/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index da9d0c2f64..863c93f7de 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -995,8 +995,9 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const robot_state::Robot collision_detection::CollisionRequest req; collision_detection::CollisionResult res; collision_detection::AllowedCollisionMatrix acm; - acm.setDefaultEntry("cone", (boost::function) - 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; From f4a7b2d3d251af65555f2485985409a4ffd3bbb7 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 6 Jun 2019 12:05:16 +0200 Subject: [PATCH 12/12] Remove not necessary stamp --- moveit_core/kinematic_constraints/src/kinematic_constraint.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 863c93f7de..0f815a1bb7 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -856,7 +856,6 @@ void VisibilityConstraint::getMarkers(const robot_state::RobotState& state, shapes::Mesh* m = getVisibilityCone(state); visualization_msgs::msg::Marker mk; shapes::constructMarkerFromShape(m, mk); - rclcpp::Time stamp = rclcpp::Clock().now(); delete m; mk.header.frame_id = robot_model_->getModelFrame(); mk.header.stamp = rclcpp::Clock().now();