From 3d16b4a0d0adaad7d5b8e054a37664b8f19c064b Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 30 Jan 2024 17:12:27 +0100 Subject: [PATCH] Remove unused python content from planning interface directory (#2665) * Remove unused python content from planning interface directory * Cleanup test cmake --- moveit_ros/planning_interface/CMakeLists.txt | 14 - .../src/wrap_python_move_group.cpp | 821 ------------------ .../wrap_python_planning_scene_interface.cpp | 137 --- .../py_bindings_tools/CMakeLists.txt | 26 - .../py_bindings_tools/dox/py_bindings.dox | 48 - .../moveit/py_bindings_tools/gil_releaser.h | 96 -- .../moveit/py_bindings_tools/py_conversions.h | 96 -- .../py_bindings_tools/roscpp_initializer.h | 74 -- .../moveit/py_bindings_tools/serialize_msg.h | 137 --- .../src/roscpp_initializer.cpp | 157 ---- .../src/wrap_python_roscpp_initializer.cpp | 53 -- .../moveit_ros_planning_interface/__init__.py | 0 .../robot_interface/CMakeLists.txt | 15 - .../src/wrap_python_robot_interface.cpp | 430 --------- moveit_ros/planning_interface/setup.py | 9 - .../planning_interface/test/CMakeLists.txt | 19 - moveit_ros/planning_interface/test/cleanup.py | 42 - .../planning_interface/test/cleanup.test | 5 - .../test/dual_arm_robot_state_update.py | 59 -- .../test/dual_arm_robot_state_update.test | 5 - .../test/python_move_group.py | 112 --- .../test/python_move_group.test | 9 - .../test/python_move_group_ns.py | 124 --- .../test/python_move_group_ns.test | 9 - .../test/robot_state_update.py | 58 -- .../test/robot_state_update.test | 5 - .../planning_interface/test/serialize_msg.py | 116 --- .../test/serialize_msg_python_cpp_helpers.cpp | 134 --- .../planning_interface/test/test_cleanup.py | 8 - 29 files changed, 2818 deletions(-) delete mode 100644 moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp delete mode 100644 moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/src/roscpp_initializer.cpp delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/src/wrap_python_roscpp_initializer.cpp delete mode 100644 moveit_ros/planning_interface/python/moveit_ros_planning_interface/__init__.py delete mode 100644 moveit_ros/planning_interface/robot_interface/CMakeLists.txt delete mode 100644 moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp delete mode 100644 moveit_ros/planning_interface/setup.py delete mode 100755 moveit_ros/planning_interface/test/cleanup.py delete mode 100644 moveit_ros/planning_interface/test/cleanup.test delete mode 100755 moveit_ros/planning_interface/test/dual_arm_robot_state_update.py delete mode 100644 moveit_ros/planning_interface/test/dual_arm_robot_state_update.test delete mode 100755 moveit_ros/planning_interface/test/python_move_group.py delete mode 100644 moveit_ros/planning_interface/test/python_move_group.test delete mode 100755 moveit_ros/planning_interface/test/python_move_group_ns.py delete mode 100644 moveit_ros/planning_interface/test/python_move_group_ns.test delete mode 100755 moveit_ros/planning_interface/test/robot_state_update.py delete mode 100644 moveit_ros/planning_interface/test/robot_state_update.test delete mode 100644 moveit_ros/planning_interface/test/serialize_msg.py delete mode 100644 moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp delete mode 100755 moveit_ros/planning_interface/test/test_cleanup.py diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 9b121ca81f..812ac85e0b 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -18,19 +18,10 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) -find_package(rclpy REQUIRED) - -# find_package(PythonInterp REQUIRED) -# find_package(PythonLibs "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" REQUIRED) find_package(Eigen3 REQUIRED) -# find_package(eigenpy REQUIRED) - -# Finds Boost Components -include(ConfigExtras.cmake) set(THIS_PACKAGE_INCLUDE_DIRS -# py_bindings_tools/include common_planning_interface_objects/include planning_scene_interface/include move_group_interface/include @@ -40,8 +31,6 @@ set(THIS_PACKAGE_LIBRARIES moveit_common_planning_interface_objects moveit_planning_scene_interface moveit_move_group_interface -# moveit_py_bindings_tools -# moveit_py_bindings_tools_python ) set(THIS_PACKAGE_INCLUDE_DEPENDS @@ -58,16 +47,13 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_action Eigen3 - Boost ) include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) -# add_subdirectory(py_bindings_tools) add_subdirectory(common_planning_interface_objects) add_subdirectory(planning_scene_interface) add_subdirectory(move_group_interface) -# add_subdirectory(robot_interface) # add_subdirectory(test) ############# diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp deleted file mode 100644 index 9c5c8dd466..0000000000 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ /dev/null @@ -1,821 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -/** @cond IGNORE */ - -namespace bp = boost::python; - -using moveit::py_bindings_tools::GILReleaser; - -namespace moveit -{ -namespace planning_interface -{ -class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer, public MoveGroupInterface -{ -public: - // ROSInitializer is constructed first, and ensures ros::init() was called, if - // needed - MoveGroupInterfaceWrapper(const std::string& group_name, const std::string& robot_description, - const std::string& ns = "", double wait_for_servers = 5.0) - : py_bindings_tools::ROScppInitializer() - , MoveGroupInterface(Options(group_name, robot_description, ros::NodeHandle(ns)), - std::shared_ptr(), ros::WallDuration(wait_for_servers)) - { - } - - bool setJointValueTargetPerJointPythonList(const std::string& joint, bp::list& values) - { - return setJointValueTarget(joint, py_bindings_tools::doubleFromList(values)); - } - - bool setJointValueTargetPythonIterable(bp::object& values) - { - return setJointValueTarget(py_bindings_tools::doubleFromList(values)); - } - - bool setJointValueTargetPythonDict(bp::dict& values) - { - bp::list k = values.keys(); - int l = bp::len(k); - std::map v; - for (int i = 0; i < l; ++i) - v[bp::extract(k[i])] = bp::extract(values[k[i]]); - return setJointValueTarget(v); - } - - bool setJointValueTargetFromPosePython(const py_bindings_tools::ByteString& pose_str, const std::string& eef, - bool approx) - { - geometry_msgs::Pose pose_msg; - py_bindings_tools::deserializeMsg(pose_str, pose_msg); - return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); - } - - bool setJointValueTargetFromPoseStampedPython(const py_bindings_tools::ByteString& pose_str, const std::string& eef, - bool approx) - { - geometry_msgs::PoseStamped pose_msg; - py_bindings_tools::deserializeMsg(pose_str, pose_msg); - return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); - } - - bool setJointValueTargetFromJointStatePython(const py_bindings_tools::ByteString& js_str) - { - sensor_msgs::JointState js_msg; - py_bindings_tools::deserializeMsg(js_str, js_msg); - return setJointValueTarget(js_msg); - } - - bp::list getJointValueTargetPythonList() - { - std::vector values; - MoveGroupInterface::getJointValueTarget(values); - bp::list l; - for (const double value : values) - l.append(value); - return l; - } - - py_bindings_tools::ByteString getJointValueTarget() - { - moveit_msgs::msg::RobotState msg; - const moveit::core::RobotState state = moveit::planning_interface::MoveGroupInterface::getTargetRobotState(); - moveit::core::robotStateToRobotStateMsg(state, msg); - return py_bindings_tools::serializeMsg(msg); - } - - void rememberJointValuesFromPythonList(const std::string& string, bp::list& values) - { - rememberJointValues(string, py_bindings_tools::doubleFromList(values)); - } - - const char* getPlanningFrameCStr() const - { - return getPlanningFrame().c_str(); - } - - py_bindings_tools::ByteString getInterfaceDescriptionPython() - { - moveit_msgs::msg::PlannerInterfaceDescription msg; - getInterfaceDescription(msg); - return py_bindings_tools::serializeMsg(msg); - } - - bp::list getActiveJointsList() const - { - return py_bindings_tools::listFromString(getActiveJoints()); - } - - bp::list getJointsList() const - { - return py_bindings_tools::listFromString(getJoints()); - } - - bp::list getCurrentJointValuesList() - { - return py_bindings_tools::listFromDouble(getCurrentJointValues()); - } - - bp::list getRandomJointValuesList() - { - return py_bindings_tools::listFromDouble(getRandomJointValues()); - } - - bp::dict getRememberedJointValuesPython() const - { - const std::map>& rv = getRememberedJointValues(); - bp::dict d; - for (const std::pair>& it : rv) - d[it.first] = py_bindings_tools::listFromDouble(it.second); - return d; - } - - bp::list convertPoseToList(const geometry_msgs::Pose& pose) const - { - std::vector v(7); - v[0] = pose.position.x; - v[1] = pose.position.y; - v[2] = pose.position.z; - v[3] = pose.orientation.x; - v[4] = pose.orientation.y; - v[5] = pose.orientation.z; - v[6] = pose.orientation.w; - return moveit::py_bindings_tools::listFromDouble(v); - } - - bp::list convertTransformToList(const geometry_msgs::Transform& tr) const - { - std::vector v(7); - v[0] = tr.translation.x; - v[1] = tr.translation.y; - v[2] = tr.translation.z; - v[3] = tr.rotation.x; - v[4] = tr.rotation.y; - v[5] = tr.rotation.z; - v[6] = tr.rotation.w; - return py_bindings_tools::listFromDouble(v); - } - - void convertListToTransform(const bp::list& l, geometry_msgs::Transform& tr) const - { - std::vector v = py_bindings_tools::doubleFromList(l); - tr.translation.x = v[0]; - tr.translation.y = v[1]; - tr.translation.z = v[2]; - tr.rotation.x = v[3]; - tr.rotation.y = v[4]; - tr.rotation.z = v[5]; - tr.rotation.w = v[6]; - } - - void convertListToPose(const bp::list& l, geometry_msgs::Pose& p) const - { - std::vector v = py_bindings_tools::doubleFromList(l); - p.position.x = v[0]; - p.position.y = v[1]; - p.position.z = v[2]; - p.orientation.x = v[3]; - p.orientation.y = v[4]; - p.orientation.z = v[5]; - p.orientation.w = v[6]; - } - - bp::list getCurrentRPYPython(const std::string& end_effector_link = "") - { - return py_bindings_tools::listFromDouble(getCurrentRPY(end_effector_link)); - } - - bp::list getCurrentPosePython(const std::string& end_effector_link = "") - { - geometry_msgs::PoseStamped pose = getCurrentPose(end_effector_link); - return convertPoseToList(pose.pose); - } - - bp::list getRandomPosePython(const std::string& end_effector_link = "") - { - geometry_msgs::PoseStamped pose = getRandomPose(end_effector_link); - return convertPoseToList(pose.pose); - } - - bp::list getKnownConstraintsList() const - { - return py_bindings_tools::listFromString(getKnownConstraints()); - } - - bool placePose(const std::string& object_name, const bp::list& pose, bool plan_only = false) - { - geometry_msgs::PoseStamped msg; - convertListToPose(pose, msg.pose); - msg.header.frame_id = getPoseReferenceFrame(); - msg.header.stamp = ros::Time::now(); - GILReleaser gr; - return place(object_name, msg, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool placePoses(const std::string& object_name, const bp::list& poses_list, bool plan_only = false) - { - int l = bp::len(poses_list); - std::vector poses(l); - for (int i = 0; i < l; ++i) - py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(poses_list[i]), poses[i]); - GILReleaser gr; - return place(object_name, poses, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool placeLocation(const std::string& object_name, const py_bindings_tools::ByteString& location_str, - bool plan_only = false) - { - std::vector locations(1); - py_bindings_tools::deserializeMsg(location_str, locations[0]); - GILReleaser gr; - return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool placeLocations(const std::string& object_name, const bp::list& location_list, bool plan_only = false) - { - int l = bp::len(location_list); - std::vector locations(l); - for (int i = 0; i < l; ++i) - py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(location_list[i]), locations[i]); - GILReleaser gr; - return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool placeAnywhere(const std::string& object_name, bool plan_only = false) - { - GILReleaser gr; - return place(object_name, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; - } - - void convertListToArrayOfPoses(const bp::list& poses, std::vector& msg) - { - int l = bp::len(poses); - for (int i = 0; i < l; ++i) - { - const bp::list& pose = bp::extract(poses[i]); - std::vector v = py_bindings_tools::doubleFromList(pose); - if (v.size() == 6 || v.size() == 7) - { - Eigen::Isometry3d p; - if (v.size() == 6) - { - tf2::Quaternion tq; - tq.setRPY(v[3], v[4], v[5]); - Eigen::Quaterniond eq; - tf2::convert(tq, eq); - p = Eigen::Isometry3d(eq); - } - else - p = Eigen::Isometry3d(Eigen::Quaterniond(v[6], v[3], v[4], v[5])); - p.translation() = Eigen::Vector3d(v[0], v[1], v[2]); - geometry_msgs::Pose pm = tf2::toMsg(p); - msg.push_back(pm); - } - else - ROS_WARN("Incorrect number of values for a pose: %u", (unsigned int)v.size()); - } - } - - bp::dict getCurrentStateBoundedPython() - { - moveit::core::RobotStatePtr current = getCurrentState(); - current->enforceBounds(); - moveit_msgs::msg::RobotState rsmv; - moveit::core::robotStateToRobotStateMsg(*current, rsmv); - bp::dict output; - for (size_t x = 0; x < rsmv.joint_state.name.size(); ++x) - output[rsmv.joint_state.name[x]] = rsmv.joint_state.position[x]; - return output; - } - - py_bindings_tools::ByteString getCurrentStatePython() - { - moveit::core::RobotStatePtr current_state = getCurrentState(); - moveit_msgs::RobotState state_message; - moveit::core::robotStateToRobotStateMsg(*current_state, state_message); - return py_bindings_tools::serializeMsg(state_message); - } - - void setStartStatePython(const py_bindings_tools::ByteString& msg_str) - { - moveit_msgs::msg::RobotState msg; - py_bindings_tools::deserializeMsg(msg_str, msg); - setStartState(msg); - } - - bool setPoseTargetsPython(bp::list& poses, const std::string& end_effector_link = "") - { - std::vector msg; - convertListToArrayOfPoses(poses, msg); - return setPoseTargets(msg, end_effector_link); - } - py_bindings_tools::ByteString getPoseTargetPython(const std::string& end_effector_link) - { - geometry_msgs::PoseStamped pose = moveit::planning_interface::MoveGroupInterface::getPoseTarget(end_effector_link); - return py_bindings_tools::serializeMsg(pose); - } - - bool setPoseTargetPython(bp::list& pose, const std::string& end_effector_link = "") - { - std::vector v = py_bindings_tools::doubleFromList(pose); - geometry_msgs::Pose msg; - if (v.size() == 6) - { - tf2::Quaternion q; - q.setRPY(v[3], v[4], v[5]); - tf2::convert(q, msg.orientation); - } - else if (v.size() == 7) - { - msg.orientation.x = v[3]; - msg.orientation.y = v[4]; - msg.orientation.z = v[5]; - msg.orientation.w = v[6]; - } - else - { - ROS_ERROR("Pose description expected to consist of either 6 or 7 values"); - return false; - } - msg.position.x = v[0]; - msg.position.y = v[1]; - msg.position.z = v[2]; - return setPoseTarget(msg, end_effector_link); - } - - const char* getEndEffectorLinkCStr() const - { - return getEndEffectorLink().c_str(); - } - - const char* getPoseReferenceFrameCStr() const - { - return getPoseReferenceFrame().c_str(); - } - - const char* getNameCStr() const - { - return getName().c_str(); - } - - const char* getPlannerIdCStr() const - { - return getPlannerId().c_str(); - } - - const char* getPlanningPipelineIdCStr() const - { - return getPlanningPipelineId().c_str(); - } - - bp::dict getNamedTargetValuesPython(const std::string& name) - { - bp::dict output; - std::map positions = getNamedTargetValues(name); - std::map::iterator iterator; - - for (iterator = positions.begin(); iterator != positions.end(); ++iterator) - output[iterator->first] = iterator->second; - return output; - } - - bp::list getNamedTargetsPython() - { - return py_bindings_tools::listFromString(getNamedTargets()); - } - - bool movePython() - { - GILReleaser gr; - return move() == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool asyncMovePython() - { - return asyncMove() == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool attachObjectPython(const std::string& object_name, const std::string& link_name, const bp::list& touch_links) - { - return attachObject(object_name, link_name, py_bindings_tools::stringFromList(touch_links)); - } - - bool executePython(const py_bindings_tools::ByteString& plan_str) - { - MoveGroupInterface::Plan plan; - py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); - GILReleaser gr; - return execute(plan) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool asyncExecutePython(const py_bindings_tools::ByteString& plan_str) - { - MoveGroupInterface::Plan plan; - py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); - return asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bp::tuple planPython() - { - MoveGroupInterface::Plan plan; - moveit_msgs::MoveItErrorCodes res; - { - GILReleaser gr; - res = MoveGroupInterface::plan(plan); - } - return bp::make_tuple(py_bindings_tools::serializeMsg(res), py_bindings_tools::serializeMsg(plan.trajectory_), - plan.planning_time); - } - - py_bindings_tools::ByteString constructMotionPlanRequestPython() - { - moveit_msgs::MotionPlanRequest request; - constructMotionPlanRequest(request); - return py_bindings_tools::serializeMsg(request); - } - - bp::tuple computeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions) - { - moveit_msgs::msg::Constraints path_constraints_tmp; - return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints_tmp); - } - - bp::tuple computeCartesianPathConstrainedPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, - const py_bindings_tools::ByteString& path_constraints_str) - { - moveit_msgs::msg::Constraints path_constraints; - py_bindings_tools::deserializeMsg(path_constraints_str, path_constraints); - return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints); - } - - bp::tuple doComputeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, const moveit_msgs::msg::Constraints& path_constraints) - { - std::vector poses; - convertListToArrayOfPoses(waypoints, poses); - moveit_msgs::msg::RobotTrajectory trajectory; - double fraction; - { - GILReleaser gr; - fraction = computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions); - } - return bp::make_tuple(py_bindings_tools::serializeMsg(trajectory), fraction); - } - - int pickGrasp(const std::string& object, const py_bindings_tools::ByteString& grasp_str, bool plan_only = false) - { - moveit_msgs::msg::Grasp grasp; - py_bindings_tools::deserializeMsg(grasp_str, grasp); - GILReleaser gr; - return pick(object, grasp, plan_only).val; - } - - int pickGrasps(const std::string& object, const bp::list& grasp_list, bool plan_only = false) - { - int l = bp::len(grasp_list); - std::vector grasps(l); - for (int i = 0; i < l; ++i) - py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(grasp_list[i]), grasps[i]); - GILReleaser gr; - return pick(object, std::move(grasps), plan_only).val; - } - - void setPathConstraintsFromMsg(const py_bindings_tools::ByteString& constraints_str) - { - moveit_msgs::msg::Constraints constraints_msg; - py_bindings_tools::deserializeMsg(constraints_str, constraints_msg); - setPathConstraints(constraints_msg); - } - - py_bindings_tools::ByteString getPathConstraintsPython() - { - moveit_msgs::msg::Constraints constraints_msg(getPathConstraints()); - return py_bindings_tools::serializeMsg(constraints_msg); - } - - void setTrajectoryConstraintsFromMsg(const py_bindings_tools::ByteString& constraints_str) - { - moveit_msgs::TrajectoryConstraints constraints_msg; - py_bindings_tools::deserializeMsg(constraints_str, constraints_msg); - setTrajectoryConstraints(constraints_msg); - } - - py_bindings_tools::ByteString getTrajectoryConstraintsPython() - { - moveit_msgs::TrajectoryConstraints constraints_msg(getTrajectoryConstraints()); - return py_bindings_tools::serializeMsg(constraints_msg); - } - - py_bindings_tools::ByteString retimeTrajectory(const py_bindings_tools::ByteString& ref_state_str, - const py_bindings_tools::ByteString& traj_str, - double velocity_scaling_factor, double acceleration_scaling_factor, - const std::string& algorithm) - { - // Convert reference state message to object - moveit_msgs::msg::RobotState ref_state_msg; - py_bindings_tools::deserializeMsg(ref_state_str, ref_state_msg); - moveit::core::RobotState ref_state_obj(getRobotModel()); - if (moveit::core::robotStateMsgToRobotState(ref_state_msg, ref_state_obj, true)) - { - // Convert trajectory message to object - moveit_msgs::msg::RobotTrajectory traj_msg; - py_bindings_tools::deserializeMsg(traj_str, traj_msg); - bool algorithm_found = true; - { - GILReleaser gr; - robot_trajectory::RobotTrajectory traj_obj(getRobotModel(), getName()); - traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg); - - // Do the actual retiming - if (algorithm == "iterative_time_parameterization") - { - trajectory_processing::IterativeParabolicTimeParameterization time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else if (algorithm == "iterative_spline_parameterization") - { - trajectory_processing::IterativeSplineParameterization time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else if (algorithm == "time_optimal_trajectory_generation") - { - trajectory_processing::TimeOptimalTrajectoryGeneration time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else - { - ROS_ERROR_STREAM_NAMED("move_group_py", "Unknown time parameterization algorithm: " << algorithm); - algorithm_found = false; - traj_msg = moveit_msgs::RobotTrajectory(); - } - - if (algorithm_found) - // Convert the retimed trajectory back into a message - traj_obj.getRobotTrajectoryMsg(traj_msg); - } - return py_bindings_tools::serializeMsg(traj_msg); - } - else - { - ROS_ERROR("Unable to convert RobotState message to RobotState instance."); - return py_bindings_tools::ByteString(""); - } - } - - Eigen::MatrixXd getJacobianMatrixPython(const bp::list& joint_values, const bp::object& reference_point = bp::object()) - { - const std::vector v = py_bindings_tools::doubleFromList(joint_values); - std::vector ref; - if (reference_point.is_none()) - ref = { 0.0, 0.0, 0.0 }; - else - ref = py_bindings_tools::doubleFromList(reference_point); - if (ref.size() != 3) - throw std::invalid_argument("reference point needs to have 3 elements, got " + std::to_string(ref.size())); - - moveit::core::RobotState state(getRobotModel()); - state.setToDefaultValues(); - auto group = state.getJointModelGroup(getName()); - state.setJointGroupPositions(group, v); - return state.getJacobian(group, Eigen::Map(&ref[0])); - } - - py_bindings_tools::ByteString enforceBoundsPython(const py_bindings_tools::ByteString& msg_str) - { - moveit_msgs::RobotState state_msg; - py_bindings_tools::deserializeMsg(msg_str, state_msg); - moveit::core::RobotState state(getRobotModel()); - if (moveit::core::robotStateMsgToRobotState(state_msg, state, true)) - { - state.enforceBounds(); - moveit::core::robotStateToRobotStateMsg(state, state_msg); - return py_bindings_tools::serializeMsg(state_msg); - } - else - { - ROS_ERROR("Unable to convert RobotState message to RobotState instance."); - return py_bindings_tools::ByteString(""); - } - } -}; - -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getJacobianMatrixOverloads, getJacobianMatrixPython, 1, 2) - -static void wrap_move_group_interface() -{ - eigenpy::enableEigenPy(); - - bp::class_ move_group_interface_class( - "MoveGroupInterface", bp::init>()); - - move_group_interface_class.def("async_move", &MoveGroupInterfaceWrapper::asyncMovePython); - move_group_interface_class.def("move", &MoveGroupInterfaceWrapper::movePython); - move_group_interface_class.def("execute", &MoveGroupInterfaceWrapper::executePython); - move_group_interface_class.def("async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython); - moveit::core::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) = - &MoveGroupInterfaceWrapper::pick; - move_group_interface_class.def("pick", pick_1); - move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasp); - move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasps); - move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placePose); - move_group_interface_class.def("place_poses_list", &MoveGroupInterfaceWrapper::placePoses); - move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeLocation); - move_group_interface_class.def("place_locations_list", &MoveGroupInterfaceWrapper::placeLocations); - move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeAnywhere); - move_group_interface_class.def("stop", &MoveGroupInterfaceWrapper::stop); - - move_group_interface_class.def("get_name", &MoveGroupInterfaceWrapper::getNameCStr); - move_group_interface_class.def("get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr); - move_group_interface_class.def("get_interface_description", &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython); - - move_group_interface_class.def("get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList); - move_group_interface_class.def("get_joints", &MoveGroupInterfaceWrapper::getJointsList); - move_group_interface_class.def("get_variable_count", &MoveGroupInterfaceWrapper::getVariableCount); - move_group_interface_class.def("allow_looking", &MoveGroupInterfaceWrapper::allowLooking); - move_group_interface_class.def("allow_replanning", &MoveGroupInterfaceWrapper::allowReplanning); - - move_group_interface_class.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); - - move_group_interface_class.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); - move_group_interface_class.def("set_end_effector_link", &MoveGroupInterfaceWrapper::setEndEffectorLink); - move_group_interface_class.def("get_end_effector_link", &MoveGroupInterfaceWrapper::getEndEffectorLinkCStr); - move_group_interface_class.def("get_pose_reference_frame", &MoveGroupInterfaceWrapper::getPoseReferenceFrameCStr); - - move_group_interface_class.def("set_pose_target", &MoveGroupInterfaceWrapper::setPoseTargetPython); - - move_group_interface_class.def("set_pose_targets", &MoveGroupInterfaceWrapper::setPoseTargetsPython); - - move_group_interface_class.def("set_position_target", &MoveGroupInterfaceWrapper::setPositionTarget); - move_group_interface_class.def("set_rpy_target", &MoveGroupInterfaceWrapper::setRPYTarget); - move_group_interface_class.def("set_orientation_target", &MoveGroupInterfaceWrapper::setOrientationTarget); - - move_group_interface_class.def("get_current_pose", &MoveGroupInterfaceWrapper::getCurrentPosePython); - move_group_interface_class.def("get_current_rpy", &MoveGroupInterfaceWrapper::getCurrentRPYPython); - - move_group_interface_class.def("get_random_pose", &MoveGroupInterfaceWrapper::getRandomPosePython); - - move_group_interface_class.def("clear_pose_target", &MoveGroupInterfaceWrapper::clearPoseTarget); - move_group_interface_class.def("clear_pose_targets", &MoveGroupInterfaceWrapper::clearPoseTargets); - - move_group_interface_class.def("set_joint_value_target", - &MoveGroupInterfaceWrapper::setJointValueTargetPythonIterable); - move_group_interface_class.def("set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonDict); - - move_group_interface_class.def("set_joint_value_target", - &MoveGroupInterfaceWrapper::setJointValueTargetPerJointPythonList); - bool (MoveGroupInterfaceWrapper::*set_joint_value_target_4)(const std::string&, double) = - &MoveGroupInterfaceWrapper::setJointValueTarget; - move_group_interface_class.def("set_joint_value_target", set_joint_value_target_4); - - move_group_interface_class.def("set_joint_value_target_from_pose", - &MoveGroupInterfaceWrapper::setJointValueTargetFromPosePython); - move_group_interface_class.def("set_joint_value_target_from_pose_stamped", - &MoveGroupInterfaceWrapper::setJointValueTargetFromPoseStampedPython); - move_group_interface_class.def("set_joint_value_target_from_joint_state_message", - &MoveGroupInterfaceWrapper::setJointValueTargetFromJointStatePython); - - move_group_interface_class.def("get_joint_value_target", &MoveGroupInterfaceWrapper::getJointValueTargetPythonList); - - move_group_interface_class.def("set_named_target", &MoveGroupInterfaceWrapper::setNamedTarget); - move_group_interface_class.def("set_random_target", &MoveGroupInterfaceWrapper::setRandomTarget); - - void (MoveGroupInterfaceWrapper::*remember_joint_values_2)(const std::string&) = - &MoveGroupInterfaceWrapper::rememberJointValues; - move_group_interface_class.def("remember_joint_values", remember_joint_values_2); - - move_group_interface_class.def("remember_joint_values", &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList); - - move_group_interface_class.def("start_state_monitor", &MoveGroupInterfaceWrapper::startStateMonitor); - move_group_interface_class.def("get_current_joint_values", &MoveGroupInterfaceWrapper::getCurrentJointValuesList); - move_group_interface_class.def("get_random_joint_values", &MoveGroupInterfaceWrapper::getRandomJointValuesList); - move_group_interface_class.def("get_remembered_joint_values", - &MoveGroupInterfaceWrapper::getRememberedJointValuesPython); - - move_group_interface_class.def("forget_joint_values", &MoveGroupInterfaceWrapper::forgetJointValues); - - move_group_interface_class.def("get_goal_joint_tolerance", &MoveGroupInterfaceWrapper::getGoalJointTolerance); - move_group_interface_class.def("get_goal_position_tolerance", &MoveGroupInterfaceWrapper::getGoalPositionTolerance); - move_group_interface_class.def("get_goal_orientation_tolerance", - &MoveGroupInterfaceWrapper::getGoalOrientationTolerance); - - move_group_interface_class.def("set_goal_joint_tolerance", &MoveGroupInterfaceWrapper::setGoalJointTolerance); - move_group_interface_class.def("set_goal_position_tolerance", &MoveGroupInterfaceWrapper::setGoalPositionTolerance); - move_group_interface_class.def("set_goal_orientation_tolerance", - &MoveGroupInterfaceWrapper::setGoalOrientationTolerance); - move_group_interface_class.def("set_goal_tolerance", &MoveGroupInterfaceWrapper::setGoalTolerance); - - move_group_interface_class.def("set_start_state_to_current_state", - &MoveGroupInterfaceWrapper::setStartStateToCurrentState); - move_group_interface_class.def("set_start_state", &MoveGroupInterfaceWrapper::setStartStatePython); - - bool (MoveGroupInterfaceWrapper::*set_path_constraints_1)(const std::string&) = - &MoveGroupInterfaceWrapper::setPathConstraints; - move_group_interface_class.def("set_path_constraints", set_path_constraints_1); - move_group_interface_class.def("set_path_constraints_from_msg", &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg); - move_group_interface_class.def("get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython); - move_group_interface_class.def("clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints); - - move_group_interface_class.def("set_trajectory_constraints_from_msg", - &MoveGroupInterfaceWrapper::setTrajectoryConstraintsFromMsg); - move_group_interface_class.def("get_trajectory_constraints", - &MoveGroupInterfaceWrapper::getTrajectoryConstraintsPython); - move_group_interface_class.def("clear_trajectory_constraints", &MoveGroupInterfaceWrapper::clearTrajectoryConstraints); - move_group_interface_class.def("get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList); - move_group_interface_class.def("set_constraints_database", &MoveGroupInterfaceWrapper::setConstraintsDatabase); - move_group_interface_class.def("set_workspace", &MoveGroupInterfaceWrapper::setWorkspace); - move_group_interface_class.def("set_planning_time", &MoveGroupInterfaceWrapper::setPlanningTime); - move_group_interface_class.def("get_planning_time", &MoveGroupInterfaceWrapper::getPlanningTime); - move_group_interface_class.def("set_max_velocity_scaling_factor", - &MoveGroupInterfaceWrapper::setMaxVelocityScalingFactor); - move_group_interface_class.def("set_max_acceleration_scaling_factor", - &MoveGroupInterfaceWrapper::setMaxAccelerationScalingFactor); - move_group_interface_class.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId); - move_group_interface_class.def("get_planner_id", &MoveGroupInterfaceWrapper::getPlannerIdCStr); - move_group_interface_class.def("set_planning_pipeline_id", &MoveGroupInterfaceWrapper::setPlanningPipelineId); - move_group_interface_class.def("get_planning_pipeline_id", &MoveGroupInterfaceWrapper::getPlanningPipelineIdCStr); - move_group_interface_class.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts); - move_group_interface_class.def("plan", &MoveGroupInterfaceWrapper::planPython); - move_group_interface_class.def("construct_motion_plan_request", - &MoveGroupInterfaceWrapper::constructMotionPlanRequestPython); - move_group_interface_class.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython); - move_group_interface_class.def("compute_cartesian_path", - &MoveGroupInterfaceWrapper::computeCartesianPathConstrainedPython); - move_group_interface_class.def("set_support_surface_name", &MoveGroupInterfaceWrapper::setSupportSurfaceName); - move_group_interface_class.def("attach_object", &MoveGroupInterfaceWrapper::attachObjectPython); - move_group_interface_class.def("detach_object", &MoveGroupInterfaceWrapper::detachObject); - move_group_interface_class.def("retime_trajectory", &MoveGroupInterfaceWrapper::retimeTrajectory); - move_group_interface_class.def("get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython); - move_group_interface_class.def("get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython); - move_group_interface_class.def("get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython); - move_group_interface_class.def("get_current_state", &MoveGroupInterfaceWrapper::getCurrentStatePython); - move_group_interface_class.def("get_jacobian_matrix", &MoveGroupInterfaceWrapper::getJacobianMatrixPython, - getJacobianMatrixOverloads()); - move_group_interface_class.def("enforce_bounds", &MoveGroupInterfaceWrapper::enforceBoundsPython); -} -} // namespace planning_interface -} // namespace moveit - -BOOST_PYTHON_MODULE(_moveit_move_group_interface) -{ - using namespace moveit::planning_interface; - wrap_move_group_interface(); -} - -/** @endcond */ diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp deleted file mode 100644 index 5455bf1ff1..0000000000 --- a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include -#include -#include - -#include -#include - -/** @cond IGNORE */ - -namespace bp = boost::python; - -namespace moveit -{ -namespace planning_interface -{ -class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitializer, public PlanningSceneInterface -{ -public: - // ROSInitializer is constructed first, and ensures ros::init() was called, if needed - PlanningSceneInterfaceWrapper(const std::string& ns = "") - : py_bindings_tools::ROScppInitializer(), PlanningSceneInterface(ns) - { - } - - bp::list getKnownObjectNamesPython(bool with_type = false) - { - return py_bindings_tools::listFromString(getKnownObjectNames(with_type)); - } - - bp::list getKnownObjectNamesInROIPython(double minx, double miny, double minz, double maxx, double maxy, double maxz, - bool with_type = false) - { - return py_bindings_tools::listFromString(getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type)); - } - - bp::dict getObjectPosesPython(const bp::list& object_ids) - { - std::map ops = getObjectPoses(py_bindings_tools::stringFromList(object_ids)); - std::map ser_ops; - for (std::map::const_iterator it = ops.begin(); it != ops.end(); ++it) - ser_ops[it->first] = py_bindings_tools::serializeMsg(it->second); - - return py_bindings_tools::dictFromType(ser_ops); - } - - bp::dict getObjectsPython(const bp::list& object_ids) - { - std::map objs = - getObjects(py_bindings_tools::stringFromList(object_ids)); - std::map ser_objs; - for (std::map::const_iterator it = objs.begin(); it != objs.end(); - ++it) - ser_objs[it->first] = py_bindings_tools::serializeMsg(it->second); - - return py_bindings_tools::dictFromType(ser_objs); - } - - bp::dict getAttachedObjectsPython(const bp::list& object_ids) - { - std::map aobjs = - getAttachedObjects(py_bindings_tools::stringFromList(object_ids)); - std::map ser_aobjs; - for (std::map::const_iterator it = aobjs.begin(); - it != aobjs.end(); ++it) - ser_aobjs[it->first] = py_bindings_tools::serializeMsg(it->second); - - return py_bindings_tools::dictFromType(ser_aobjs); - } - - bool applyPlanningScenePython(const py_bindings_tools::ByteString& ps_str) - { - moveit_msgs::msg::PlanningScene ps_msg; - py_bindings_tools::deserializeMsg(ps_str, ps_msg); - return applyPlanningScene(ps_msg); - } -}; - -static void wrap_planning_scene_interface() -{ - bp::class_ planning_scene_class("PlanningSceneInterface", - bp::init>()); - - planning_scene_class.def("get_known_object_names", &PlanningSceneInterfaceWrapper::getKnownObjectNamesPython); - planning_scene_class.def("get_known_object_names_in_roi", - &PlanningSceneInterfaceWrapper::getKnownObjectNamesInROIPython); - planning_scene_class.def("get_object_poses", &PlanningSceneInterfaceWrapper::getObjectPosesPython); - planning_scene_class.def("get_objects", &PlanningSceneInterfaceWrapper::getObjectsPython); - planning_scene_class.def("get_attached_objects", &PlanningSceneInterfaceWrapper::getAttachedObjectsPython); - planning_scene_class.def("apply_planning_scene", &PlanningSceneInterfaceWrapper::applyPlanningScenePython); -} -} // namespace planning_interface -} // namespace moveit - -BOOST_PYTHON_MODULE(_moveit_planning_scene_interface) -{ - using namespace moveit::planning_interface; - wrap_planning_scene_interface(); -} - -/** @endcond */ diff --git a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt b/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt deleted file mode 100644 index 07739486f8..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -add_library(moveit_py_bindings_tools src/roscpp_initializer.cpp) -set_target_properties(moveit_py_bindings_tools PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_py_bindings_tools ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PYTHON_LIBRARIES}) - -install(TARGETS moveit_py_bindings_tools - EXPORT moveit_py_bindings_tools - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -add_library(moveit_py_bindings_tools_python src/wrap_python_roscpp_initializer.cpp) -target_link_libraries(moveit_py_bindings_tools_python moveit_py_bindings_tools ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -set_target_properties(moveit_py_bindings_tools_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -set_target_properties(moveit_py_bindings_tools_python PROPERTIES OUTPUT_NAME _moveit_roscpp_initializer PREFIX "") -set_target_properties(moveit_py_bindings_tools_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") -if(WIN32) - set_target_properties(moveit_py_bindings_tools_python PROPERTIES SUFFIX .pyd) -endif() - -install(TARGETS moveit_py_bindings_tools_python - EXPORT moveit_py_bindings_tools_python - DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} -) - -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox b/moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox deleted file mode 100644 index 061314d0d8..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox +++ /dev/null @@ -1,48 +0,0 @@ -/**\page py_bindings_roscpp Creating Python bindings for C++ classes - - \par Problem - When creating Python bindings of C++ classes it may be - necessary to have roscpp initialized (ros::init() called) when the - instance of a particular C++ class is created. - \par Example - For example, say we need python bindings for class Foo; - A common approach to define functionality specific for the wrappers is to - define: - \code - class FooWrapper : public Foo - { - public: - ... - }; - \endcode - This allows defining additional functions in FooWrapper that are - more amenable to the construction of Python bindings. - Say that FooWrapper is now constructed because a Python programmer - created the instance of the wrapped object. At that point, if Foo - needs ros::init() to have been called, there is an error, because - even if rospy is initialized, roscpp is not. - @par Solutions - We can update class FooWrapper using the moveit_py_bindings_tools::ROScppInitializer class - like so: - \code - class FooWrapper : protected ROScppInitializer - public Foo - { - public: - FooWrapper(...) : ROScppInitializer(), - Foo(...) - { } - ... - }; - \endcode - This way it is ensured that the constructor of ROScppInitialier is - called right before the instance of Foo is constructed (in the - process of constructing FooWrapper). - The advantage of this solution is that the Python programmer could not care less - what roscpp is doing. Things will "just work". - The downside of this solution is that if any command line arguments - have been passed to the Python program, they are not forwarded to roscpp. - But to forward these parameters an explicit function call needs to be made. - For that we provide a function call - -*/ diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h deleted file mode 100644 index e7dfe3ecc3..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h +++ /dev/null @@ -1,96 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, RWTH Aachen University - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of RWTH Aachen University nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Bjarne von Horn */ - -#pragma once - -#include -#include - -namespace moveit -{ -namespace py_bindings_tools -{ -/** \brief RAII Helper to release the Global Interpreter Lock (GIL) - * - * Use this helper class to release the GIL before doing long computations - * or blocking calls. Note that without the GIL Python-related functions must not be called. - * So, before releasing the GIL all \c boost::python variables have to be converted to e.g. \c std::vector. - * Before converting the result back to e.g. a moveit::py_bindings_tools::ByteString instance, the GIL has to be - * reacquired. - */ -class GILReleaser -{ - PyThreadState* thread_state_; - -public: - /** \brief Release the GIL on construction */ - GILReleaser() noexcept - { - thread_state_ = PyEval_SaveThread(); - } - /** \brief Reacquire the GIL on destruction */ - ~GILReleaser() noexcept - { - if (thread_state_) - { - PyEval_RestoreThread(thread_state_); - thread_state_ = nullptr; - } - } - - GILReleaser(const GILReleaser&) = delete; - GILReleaser(GILReleaser&& other) noexcept - { - thread_state_ = other.thread_state_; - other.thread_state_ = nullptr; - } - - GILReleaser& operator=(const GILReleaser&) = delete; - GILReleaser& operator=(GILReleaser&& other) noexcept - { - GILReleaser copy(std::move(other)); - swap(copy); - return *this; - } - - void swap(GILReleaser& other) noexcept - { - std::swap(other.thread_state_, thread_state_); - } -}; - -} // namespace py_bindings_tools -} // namespace moveit diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h deleted file mode 100644 index acbbde0d2b..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h +++ /dev/null @@ -1,96 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace moveit -{ -namespace py_bindings_tools -{ -template -std::vector typeFromList(const boost::python::object& values) -{ - boost::python::stl_input_iterator begin(values), end; - std::vector v; - v.assign(begin, end); - return v; -} - -template -boost::python::list listFromType(const std::vector& v) -{ - boost::python::list l; - for (std::size_t i = 0; i < v.size(); ++i) - l.append(v[i]); - return l; -} - -template -boost::python::dict dictFromType(const std::map& v) -{ - boost::python::dict d; - for (typename std::map::const_iterator it = v.begin(); it != v.end(); ++it) - d[it->first] = it->second; - return d; -} - -std::vector doubleFromList(const boost::python::object& values) -{ - return typeFromList(values); -} - -std::vector stringFromList(const boost::python::object& values) -{ - return typeFromList(values); -} - -boost::python::list listFromDouble(const std::vector& v) -{ - return listFromType(v); -} - -boost::python::list listFromString(const std::vector& v) -{ - return listFromType(v); -} -} // namespace py_bindings_tools -} // namespace moveit diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h deleted file mode 100644 index f658e6a440..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h +++ /dev/null @@ -1,74 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#pragma once - -#include -#include - -namespace moveit -{ -/** \brief Tools for creating python bindings for MoveIt */ -namespace py_bindings_tools -{ -/** \brief The constructor of this class ensures that ros::init() has - been called. Thread safety and multiple initialization is - properly handled. When the process terminates, ros::shutdown() is - also called, if needed. */ -class ROScppInitializer -{ -public: - ROScppInitializer(); - ROScppInitializer(boost::python::list& argv); - ROScppInitializer(const std::string& node_name, boost::python::list& argv); -}; - -/** \brief This function can be used to specify the ROS command line arguments for the internal ROScpp instance; - Usually this function would also be exposed in the py module that uses ROScppInitializer. */ -void roscpp_set_arguments(const std::string& node_name, boost::python::list& argv); - -/** \brief Initialize ROScpp with specified command line args */ -void roscpp_init(const std::string& node_name, boost::python::list& argv); - -/** \brief Initialize ROScpp with specified command line args */ -void roscpp_init(boost::python::list& argv); - -/** \brief Initialize ROScpp with default command line args */ -void roscpp_init(); - -void roscpp_shutdown(); -} // namespace py_bindings_tools -} // namespace moveit diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h deleted file mode 100644 index 61514fdb60..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h +++ /dev/null @@ -1,137 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace moveit -{ -namespace py_bindings_tools -{ -/** \brief C++ Wrapper class for Python 3 \c Bytes Object */ -class ByteString : public boost::python::object -{ -public: - // constructors for bp::handle and friends - BOOST_PYTHON_FORWARD_OBJECT_CONSTRUCTORS(ByteString, boost::python::object) - ByteString() : boost::python::object(boost::python::handle<>(PyBytes_FromString(""))) - { - } - explicit ByteString(const char* s) : boost::python::object(boost::python::handle<>(PyBytes_FromString(s))) - { - } - explicit ByteString(const std::string& s) - : boost::python::object(boost::python::handle<>(PyBytes_FromStringAndSize(s.c_str(), s.size()))) - { - } - // bp::list[] returns a proxy which has to be converted to an object first - template - explicit ByteString(const boost::python::api::proxy& proxy) : boost::python::object(proxy) - { - } - /** \brief Serializes a ROS message into a Python Bytes object - * The second template parameter ensures that this overload is only chosen with a ROS message argument - */ - template ::value, int>::type = 0> - explicit ByteString(const T& msg) - : boost::python::object( - boost::python::handle<>(PyBytes_FromStringAndSize(nullptr, ros::serialization::serializationLength(msg)))) - { - ros::serialization::OStream stream_arg(reinterpret_cast(PyBytes_AS_STRING(ptr())), - PyBytes_GET_SIZE(ptr())); - ros::serialization::serialize(stream_arg, msg); - } - - /** \brief Convert content to a ROS message */ - template - void deserialize(T& msg) const - { - static_assert(sizeof(uint8_t) == sizeof(char), "ros/python buffer layout mismatch"); - char* buf = PyBytes_AsString(ptr()); - // buf == nullptr on error - if (!buf) - { - throw std::runtime_error("Underlying python object is not a Bytes/String instance"); - } - // unfortunately no constructor with const uint8_t - ros::serialization::IStream stream_arg(reinterpret_cast(buf), PyBytes_GET_SIZE(ptr())); - ros::serialization::deserialize(stream_arg, msg); - } -}; - -/** \brief Convert a ROS message to a Python Bytestring */ -template -ByteString serializeMsg(const T& msg) -{ - return ByteString(msg); -} - -/** \brief Convert a Python Bytestring to a ROS message */ -template -void deserializeMsg(const ByteString& data, T& msg) -{ - data.deserialize(msg); -} - -} // namespace py_bindings_tools -} // namespace moveit - -namespace boost -{ -namespace python -{ -namespace converter -{ -// only accept Python 3 Bytes / Python 2 String instance when used as C++ function parameter -template <> -struct object_manager_traits -#if PY_VERSION_HEX >= 0x03000000 - : pytype_object_manager_traits<&PyBytes_Type, moveit::py_bindings_tools::ByteString> -#else - : pytype_object_manager_traits<&PyString_Type, moveit::py_bindings_tools::ByteString> -#endif - -{ -}; -} // namespace converter -} // namespace python -} // namespace boost diff --git a/moveit_ros/planning_interface/py_bindings_tools/src/roscpp_initializer.cpp b/moveit_ros/planning_interface/py_bindings_tools/src/roscpp_initializer.cpp deleted file mode 100644 index b0a0dccf0a..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/src/roscpp_initializer.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include -#include -#include -#include - -static std::vector& ROScppArgs() -{ - static std::vector args; - return args; -} - -static std::string& ROScppNodeName() -{ - static std::string node_name("moveit_python_wrappers"); - return node_name; -} - -void moveit::py_bindings_tools::roscpp_set_arguments(const std::string& node_name, boost::python::list& argv) -{ - ROScppNodeName() = node_name; - ROScppArgs() = stringFromList(argv); -} - -namespace -{ -struct InitProxy -{ - InitProxy() - { - const std::vector& args = ROScppArgs(); - int fake_argc = args.size(); - char** fake_argv = new char*[args.size()]; - for (std::size_t i = 0; i < args.size(); ++i) - fake_argv[i] = strdup(args[i].c_str()); - - ros::init(fake_argc, fake_argv, ROScppNodeName(), - ros::init_options::AnonymousName | ros::init_options::NoSigintHandler); - for (int i = 0; i < fake_argc; ++i) - delete[] fake_argv[i]; - delete[] fake_argv; - } - - ~InitProxy() - { - if (ros::isInitialized() && !ros::isShuttingDown()) - ros::shutdown(); - } -}; -} // namespace - -static void roscpp_init_or_stop(bool init) -{ - // ensure we do not accidentally initialize ROS multiple times per process - static std::mutex lock; - std::mutex::scoped_lock slock(lock); - - // once per process, we start a spinner - static bool once = true; - static std::unique_ptr proxy; - static std::unique_ptr spinner; - - // initialize only once - if (once && init) - { - once = false; - - // if ROS (cpp) is not initialized, we initialize it - if (!ros::isInitialized()) - { - proxy = std::make_unique(); - spinner = std::make_unique(1); - spinner->start(); - } - } - - // shutdown if needed - if (!init) - { - once = false; - proxy.reset(); - spinner.reset(); - } -} - -void moveit::py_bindings_tools::roscpp_init() -{ - roscpp_init_or_stop(true); -} - -void moveit::py_bindings_tools::roscpp_init(const std::string& node_name, boost::python::list& argv) -{ - roscpp_set_arguments(node_name, argv); - roscpp_init(); -} - -void moveit::py_bindings_tools::roscpp_init(boost::python::list& argv) -{ - ROScppArgs() = stringFromList(argv); - roscpp_init(); -} - -void moveit::py_bindings_tools::roscpp_shutdown() -{ - roscpp_init_or_stop(false); -} - -moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer() -{ - roscpp_init(); -} - -moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer(boost::python::list& argv) -{ - roscpp_init(argv); -} - -moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer(const std::string& node_name, boost::python::list& argv) -{ - roscpp_init(node_name, argv); -} diff --git a/moveit_ros/planning_interface/py_bindings_tools/src/wrap_python_roscpp_initializer.cpp b/moveit_ros/planning_interface/py_bindings_tools/src/wrap_python_roscpp_initializer.cpp deleted file mode 100644 index d4cf4d4c85..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/src/wrap_python_roscpp_initializer.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include -#include - -namespace bp = boost::python; - -static void wrap_roscpp_initializer() -{ - void (*init_fn)(const std::string&, bp::list&) = &moveit::py_bindings_tools::roscpp_init; - bp::def("roscpp_init", init_fn); - bp::def("roscpp_shutdown", &moveit::py_bindings_tools::roscpp_shutdown); -} - -BOOST_PYTHON_MODULE(_moveit_roscpp_initializer) -{ - wrap_roscpp_initializer(); -} diff --git a/moveit_ros/planning_interface/python/moveit_ros_planning_interface/__init__.py b/moveit_ros/planning_interface/python/moveit_ros_planning_interface/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_ros/planning_interface/robot_interface/CMakeLists.txt b/moveit_ros/planning_interface/robot_interface/CMakeLists.txt deleted file mode 100644 index 031b350854..0000000000 --- a/moveit_ros/planning_interface/robot_interface/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -add_library(moveit_robot_interface_python src/wrap_python_robot_interface.cpp) -target_link_libraries(moveit_robot_interface_python ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} - moveit_common_planning_interface_objects moveit_py_bindings_tools -) -set_target_properties(moveit_robot_interface_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -set_target_properties(moveit_robot_interface_python PROPERTIES OUTPUT_NAME _moveit_robot_interface PREFIX "") -set_target_properties(moveit_robot_interface_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") -if(WIN32) - set_target_properties(moveit_robot_interface_python PROPERTIES SUFFIX .pyd) -endif() - -install(TARGETS moveit_robot_interface_python - EXPORT moveit_robot_interface_python - DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} -) diff --git a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp deleted file mode 100644 index 8005f5d886..0000000000 --- a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp +++ /dev/null @@ -1,430 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -/** @cond IGNORE */ - -namespace bp = boost::python; -using moveit::py_bindings_tools::GILReleaser; - -namespace moveit -{ -class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer -{ -public: - RobotInterfacePython(const std::string& robot_description, const std::string& ns = "") - : py_bindings_tools::ROScppInitializer() - { - robot_model_ = planning_interface::getSharedRobotModel(robot_description); - if (!robot_model_) - throw std::runtime_error("RobotInterfacePython: invalid robot model"); - current_state_monitor_ = - planning_interface::getSharedStateMonitor(robot_model_, planning_interface::getSharedTF(), ns); - } - - const char* getRobotName() const - { - return robot_model_->getName().c_str(); - } - - bp::list getActiveJointNames() const - { - return py_bindings_tools::listFromString(robot_model_->getActiveJointModelNames()); - } - - bp::list getGroupActiveJointNames(const std::string& group) const - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (jmg) - return py_bindings_tools::listFromString(jmg->getActiveJointModelNames()); - else - return bp::list(); - } - - bp::list getJointNames() const - { - return py_bindings_tools::listFromString(robot_model_->getJointModelNames()); - } - - bp::list getGroupJointNames(const std::string& group) const - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (jmg) - return py_bindings_tools::listFromString(jmg->getJointModelNames()); - else - return bp::list(); - } - - bp::list getGroupJointTips(const std::string& group) const - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (jmg) - { - std::vector tips; - jmg->getEndEffectorTips(tips); - return py_bindings_tools::listFromString(tips); - } - else - return bp::list(); - } - - bp::list getLinkNames() const - { - return py_bindings_tools::listFromString(robot_model_->getLinkModelNames()); - } - - bp::list getGroupLinkNames(const std::string& group) const - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (jmg) - return py_bindings_tools::listFromString(jmg->getLinkModelNames()); - else - return bp::list(); - } - - bp::list getGroupNames() const - { - return py_bindings_tools::listFromString(robot_model_->getJointModelGroupNames()); - } - - bp::list getJointLimits(const std::string& name) const - { - bp::list result; - const moveit::core::JointModel* jm = robot_model_->getJointModel(name); - if (jm) - { - const std::vector& lim = jm->getVariableBoundsMsg(); - for (const moveit_msgs::msg::JointLimits& joint_limit : lim) - { - bp::list l; - l.append(joint_limit.min_position); - l.append(joint_limit.max_position); - result.append(l); - } - } - return result; - } - - const char* getPlanningFrame() const - { - return robot_model_->getModelFrame().c_str(); - } - - bp::list getLinkPose(const std::string& name) - { - bp::list l; - if (!ensureCurrentState()) - return l; - moveit::core::RobotStatePtr state = current_state_monitor_->getCurrentState(); - const moveit::core::LinkModel* lm = state->getLinkModel(name); - if (lm) - { - // getGlobalLinkTransform() returns a valid isometry by contract - const Eigen::Isometry3d& t = state->getGlobalLinkTransform(lm); - std::vector v(7); - v[0] = t.translation().x(); - v[1] = t.translation().y(); - v[2] = t.translation().z(); - Eigen::Quaterniond q(t.linear()); - v[3] = q.x(); - v[4] = q.y(); - v[5] = q.z(); - v[6] = q.w(); - l = py_bindings_tools::listFromDouble(v); - } - return l; - } - - bp::list getDefaultStateNames(const std::string& group) - { - bp::list l; - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (jmg) - { - for (auto& known_state : jmg->getDefaultStateNames()) - { - l.append(known_state); - } - } - return l; - } - - bp::list getCurrentJointValues(const std::string& name) - { - bp::list l; - if (!ensureCurrentState()) - return l; - moveit::core::RobotStatePtr state = current_state_monitor_->getCurrentState(); - const moveit::core::JointModel* jm = state->getJointModel(name); - if (jm) - { - const double* pos = state->getJointPositions(jm); - const unsigned int sz = jm->getVariableCount(); - for (unsigned int i = 0; i < sz; ++i) - l.append(pos[i]); - } - - return l; - } - - bp::dict getJointValues(const std::string& group, const std::string& named_state) - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (!jmg) - return boost::python::dict(); - std::map values; - jmg->getVariableDefaultPositions(named_state, values); - return py_bindings_tools::dictFromType(values); - } - - bool ensureCurrentState(double wait = 1.0) - { - if (!current_state_monitor_) - { - ROS_ERROR("Unable to get current robot state"); - return false; - } - - // if needed, start the monitor and wait up to 1 second for a full robot state - if (!current_state_monitor_->isActive()) - { - GILReleaser gr; - current_state_monitor_->startStateMonitor(); - if (!current_state_monitor_->waitForCompleteState(wait)) - ROS_WARN("Joint values for monitored state are requested but the full state is not known"); - } - return true; - } - - py_bindings_tools::ByteString getCurrentState() - { - if (!ensureCurrentState()) - return py_bindings_tools::ByteString(""); - moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); - moveit_msgs::msg::RobotState msg; - moveit::core::robotStateToRobotStateMsg(*s, msg); - return py_bindings_tools::serializeMsg(msg); - } - - bp::tuple getEndEffectorParentGroup(const std::string& group) - { - // name of the group that is parent to this end-effector group; - // Second: the link this in the parent group that this group attaches to - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (!jmg) - return boost::python::make_tuple("", ""); - std::pair parent_group = jmg->getEndEffectorParentGroup(); - return boost::python::make_tuple(parent_group.first, parent_group.second); - } - - py_bindings_tools::ByteString getRobotMarkersPythonDictList(bp::dict& values, bp::list& links) - { - moveit::core::RobotStatePtr state; - if (ensureCurrentState()) - { - state = current_state_monitor_->getCurrentState(); - } - else - { - state = std::make_shared(robot_model_); - } - - bp::list k = values.keys(); - int l = bp::len(k); - sensor_msgs::JointState joint_state; - joint_state.name.resize(l); - joint_state.position.resize(l); - for (int i = 0; i < l; ++i) - { - joint_state.name[i] = bp::extract(k[i]); - joint_state.position[i] = bp::extract(values[k[i]]); - } - state->setVariableValues(joint_state); - visualization_msgs::MarkerArray msg; - state->getRobotMarkers(msg, py_bindings_tools::stringFromList(links)); - - return py_bindings_tools::serializeMsg(msg); - } - - py_bindings_tools::ByteString getRobotMarkersPythonDict(bp::dict& values) - { - bp::list links = py_bindings_tools::listFromString(robot_model_->getLinkModelNames()); - return getRobotMarkersPythonDictList(values, links); - } - - py_bindings_tools::ByteString getRobotMarkersFromMsg(const py_bindings_tools::ByteString& state_str) - { - moveit_msgs::msg::RobotState state_msg; - moveit::core::RobotState state(robot_model_); - py_bindings_tools::deserializeMsg(state_str, state_msg); - moveit::core::robotStateMsgToRobotState(state_msg, state); - - visualization_msgs::MarkerArray msg; - state.getRobotMarkers(msg, state.getRobotModel()->getLinkModelNames()); - - return py_bindings_tools::serializeMsg(msg); - } - - py_bindings_tools::ByteString getRobotMarkers() - { - if (!ensureCurrentState()) - return py_bindings_tools::ByteString(); - moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); - visualization_msgs::MarkerArray msg; - s->getRobotMarkers(msg, s->getRobotModel()->getLinkModelNames()); - - return py_bindings_tools::serializeMsg(msg); - } - - py_bindings_tools::ByteString getRobotMarkersPythonList(const bp::list& links) - { - if (!ensureCurrentState()) - return py_bindings_tools::ByteString(""); - moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); - visualization_msgs::MarkerArray msg; - s->getRobotMarkers(msg, py_bindings_tools::stringFromList(links)); - - return py_bindings_tools::serializeMsg(msg); - } - - py_bindings_tools::ByteString getRobotMarkersGroup(const std::string& group) - { - if (!ensureCurrentState()) - return py_bindings_tools::ByteString(""); - moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - visualization_msgs::MarkerArray msg; - if (jmg) - { - s->getRobotMarkers(msg, jmg->getLinkModelNames()); - } - - return py_bindings_tools::serializeMsg(msg); - } - - py_bindings_tools::ByteString getRobotMarkersGroupPythonDict(const std::string& group, bp::dict& values) - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (!jmg) - return py_bindings_tools::ByteString(""); - bp::list links = py_bindings_tools::listFromString(jmg->getLinkModelNames()); - return getRobotMarkersPythonDictList(values, links); - } - - bp::dict getCurrentVariableValues() - { - bp::dict d; - - if (!ensureCurrentState()) - return d; - - const std::map& vars = current_state_monitor_->getCurrentStateValues(); - for (const std::pair& var : vars) - d[var.first] = var.second; - - return d; - } - - const char* getRobotRootLink() const - { - return robot_model_->getRootLinkName().c_str(); - } - - bool hasGroup(const std::string& group) const - { - return robot_model_->hasJointModelGroup(group); - } - -private: - moveit::core::RobotModelConstPtr robot_model_; - planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; - ros::NodeHandle nh_; -}; -} // namespace moveit - -static void wrap_robot_interface() -{ - using namespace moveit; - - bp::class_ robot_class("RobotInterface", bp::init>()); - - robot_class.def("get_joint_names", &RobotInterfacePython::getJointNames); - robot_class.def("get_group_joint_names", &RobotInterfacePython::getGroupJointNames); - robot_class.def("get_active_joint_names", &RobotInterfacePython::getActiveJointNames); - robot_class.def("get_group_active_joint_names", &RobotInterfacePython::getGroupActiveJointNames); - robot_class.def("get_group_default_states", &RobotInterfacePython::getDefaultStateNames); - robot_class.def("get_group_joint_tips", &RobotInterfacePython::getGroupJointTips); - robot_class.def("get_group_names", &RobotInterfacePython::getGroupNames); - robot_class.def("get_link_names", &RobotInterfacePython::getLinkNames); - robot_class.def("get_group_link_names", &RobotInterfacePython::getGroupLinkNames); - robot_class.def("get_joint_limits", &RobotInterfacePython::getJointLimits); - robot_class.def("get_link_pose", &RobotInterfacePython::getLinkPose); - robot_class.def("get_planning_frame", &RobotInterfacePython::getPlanningFrame); - robot_class.def("get_current_state", &RobotInterfacePython::getCurrentState); - robot_class.def("get_current_variable_values", &RobotInterfacePython::getCurrentVariableValues); - robot_class.def("get_current_joint_values", &RobotInterfacePython::getCurrentJointValues); - robot_class.def("get_joint_values", &RobotInterfacePython::getJointValues); - robot_class.def("get_robot_root_link", &RobotInterfacePython::getRobotRootLink); - robot_class.def("has_group", &RobotInterfacePython::hasGroup); - robot_class.def("get_robot_name", &RobotInterfacePython::getRobotName); - robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkers); - robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonList); - robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersFromMsg); - robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDictList); - robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDict); - robot_class.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroup); - robot_class.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroupPythonDict); - robot_class.def("get_parent_group", &RobotInterfacePython::getEndEffectorParentGroup); -} - -BOOST_PYTHON_MODULE(_moveit_robot_interface) -{ - wrap_robot_interface(); -} - -/** @endcond */ diff --git a/moveit_ros/planning_interface/setup.py b/moveit_ros/planning_interface/setup.py deleted file mode 100644 index 4be294ccc8..0000000000 --- a/moveit_ros/planning_interface/setup.py +++ /dev/null @@ -1,9 +0,0 @@ -from setuptools import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup() -d["packages"] = ["moveit_ros_planning_interface"] -d["scripts"] = [] -d["package_dir"] = {"": "python"} - -setup(**d) diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 613f506800..0dba4fb41e 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -25,23 +25,4 @@ if(BUILD_TESTING) # add_rostest_gtest(subframes_test subframes_test.test subframes_test.cpp) # target_link_libraries(subframes_test moveit_move_group_interface # ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) - - # add_rostest(python_move_group.test) - # add_rostest(python_move_group_ns.test) - # add_rostest(robot_state_update.test) - # add_rostest(dual_arm_robot_state_update.test) - # add_rostest(cleanup.test) - - - # SET(HELPER_LIB moveit_planning_interface_test_serialize_msg_cpp_helper) - # add_library(${HELPER_LIB} serialize_msg_python_cpp_helpers.cpp) - # set_target_properties(${HELPER_LIB} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - # target_link_libraries(${HELPER_LIB} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) - # set_target_properties(${HELPER_LIB} PROPERTIES OUTPUT_NAME "_${HELPER_LIB}" PREFIX "") - # set_target_properties(${HELPER_LIB} PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") - # if(WIN32) - # set_target_properties(${HELPER_LIB} PROPERTIES SUFFIX .pyd) - # endif(WIN32) - - # catkin_add_nosetests(serialize_msg.py) endif() diff --git a/moveit_ros/planning_interface/test/cleanup.py b/moveit_ros/planning_interface/test/cleanup.py deleted file mode 100755 index 933d763756..0000000000 --- a/moveit_ros/planning_interface/test/cleanup.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python - -from __future__ import print_function - -import unittest -import rostest -import subprocess -import rospkg -import roslib.packages - -PKGNAME = "moveit_ros_planning_interface" -NODENAME = "moveit_cleanup_tests" - - -# As issue #592 is related to a crash during program exit, -# we cannot perform a standard unit test. -# We have to check the return value of the called program. -# As rostest doesn't do this automatically, we do it ourselves -# and call the actual test program here. -class CleanupTest(unittest.TestCase): - def __init__(self, *args, **kwargs): - super(CleanupTest, self).__init__(*args, **kwargs) - self._rospack = rospkg.RosPack() - - def run_cmd(self, cmd, num=5): - failures = 0 - for i in range(num): - if subprocess.call(cmd) != 0: - failures += 1 - self.assertEqual(failures, 0, "%d of %d runs failed" % (failures, num)) - - def test_py(self): - self.run_cmd( - roslib.packages.find_node(PKGNAME, "test_cleanup.py", self._rospack) - ) - - def test_cpp(self): - self.run_cmd(roslib.packages.find_node(PKGNAME, "test_cleanup", self._rospack)) - - -if __name__ == "__main__": - rostest.rosrun(PKGNAME, NODENAME, CleanupTest) diff --git a/moveit_ros/planning_interface/test/cleanup.test b/moveit_ros/planning_interface/test/cleanup.test deleted file mode 100644 index 775ea18666..0000000000 --- a/moveit_ros/planning_interface/test/cleanup.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/moveit_ros/planning_interface/test/dual_arm_robot_state_update.py b/moveit_ros/planning_interface/test/dual_arm_robot_state_update.py deleted file mode 100755 index 9486105f9a..0000000000 --- a/moveit_ros/planning_interface/test/dual_arm_robot_state_update.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python - -import unittest -import rospy -import rostest -import os - -import moveit_commander - -from moveit_msgs.msg import MoveItErrorCodes, RobotTrajectory - - -class RobotStateUpdateTest(unittest.TestCase): - @classmethod - def setUpClass(self): - self.dual_arm_group = moveit_commander.MoveGroupCommander("dual_arm") - self.panda_1_group = moveit_commander.MoveGroupCommander("panda_1") - self.panda_2_group = moveit_commander.MoveGroupCommander("panda_2") - - @classmethod - def tearDown(self): - pass - - def plan(self, target): - self.dual_arm_group.set_joint_value_target(target) - return self.dual_arm_group.plan() - - def test(self): - panda_1_pose = self.panda_1_group.get_current_pose("panda_1_link8") - panda_1_pose.pose.position.z -= 0.05 - panda_1_pose.pose.position.x += 0.05 - - panda_2_pose = self.panda_2_group.get_current_pose("panda_2_link8") - panda_2_pose.pose.position.z -= 0.05 - panda_2_pose.pose.position.x -= 0.05 - self.dual_arm_group.set_start_state_to_current_state() - self.dual_arm_group.set_pose_target( - panda_1_pose, end_effector_link="panda_1_link8" - ) - self.dual_arm_group.set_pose_target( - panda_2_pose, end_effector_link="panda_2_link8" - ) - - result = self.dual_arm_group.plan() - if isinstance(result, RobotTrajectory): - self.assertTrue(result.joint_trajectory.joint_names) - else: - success, plan, planning_time, error = result - self.assertTrue(error.val == MoveItErrorCodes.SUCCESS) - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_robot_state_update" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, RobotStateUpdateTest) - - # suppress cleanup segfault in ROS < Kinetic - os._exit(0) diff --git a/moveit_ros/planning_interface/test/dual_arm_robot_state_update.test b/moveit_ros/planning_interface/test/dual_arm_robot_state_update.test deleted file mode 100644 index de9379609e..0000000000 --- a/moveit_ros/planning_interface/test/dual_arm_robot_state_update.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/moveit_ros/planning_interface/test/python_move_group.py b/moveit_ros/planning_interface/test/python_move_group.py deleted file mode 100755 index 8b5c9a9c05..0000000000 --- a/moveit_ros/planning_interface/test/python_move_group.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python - -import unittest -import numpy as np -import rospy -import rostest -import os - -from moveit_ros_planning_interface._moveit_move_group_interface import ( - MoveGroupInterface, -) -from moveit_msgs.msg import MoveItErrorCodes - - -class PythonMoveGroupTest(unittest.TestCase): - PLANNING_GROUP = "manipulator" - - @classmethod - def setUpClass(self): - self.group = MoveGroupInterface( - self.PLANNING_GROUP, "robot_description", rospy.get_namespace() - ) - - @classmethod - def tearDown(self): - pass - - def check_target_setting(self, expect, *args): - if len(args) == 0: - args = [expect] - self.group.set_joint_value_target(*args) - res = self.group.get_joint_value_target() - self.assertTrue( - np.all(np.asarray(res) == np.asarray(expect)), - "Setting failed for %s, values: %s" % (type(args[0]), res), - ) - - def test_target_setting(self): - n = self.group.get_variable_count() - self.check_target_setting([0.1] * n) - self.check_target_setting((0.2,) * n) - self.check_target_setting(np.zeros(n)) - self.check_target_setting( - [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()} - ) - self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) - - def plan(self, target): - self.group.set_joint_value_target(target) - return self.group.plan() - - def test_validation(self): - current = np.asarray(self.group.get_current_joint_values()) - - error_code1, plan1, time = self.plan(current + 0.2) - error_code2, plan2, time = self.plan(current + 0.2) - - # both plans should have succeeded: - error_code = MoveItErrorCodes() - error_code.deserialize(error_code1) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code2) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - - # first plan should execute - self.assertTrue(self.group.execute(plan1)) - - # second plan should be invalid now (due to modified start point) and rejected - self.assertFalse(self.group.execute(plan2)) - - # newly planned trajectory should execute again - error_code3, plan3, time = self.plan(current) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code3) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - self.assertTrue(self.group.execute(plan3)) - - def test_get_jacobian_matrix(self): - current = self.group.get_current_joint_values() - result = self.group.get_jacobian_matrix(current) - # Value check by known value at the initial pose - expected = np.array( - [ - [0.0, 0.8, -0.2, 0.0, 0.0, 0.0], - [0.89, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, -0.74, 0.74, 0.0, 0.1, 0.0], - [0.0, 0.0, 0.0, -1.0, 0.0, -1.0], - [0.0, 1.0, -1.0, 0.0, -1.0, 0.0], - [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], - ] - ) - self.assertTrue(np.allclose(result, expected)) - - result = self.group.get_jacobian_matrix(current, [1.0, 1.0, 1.0]) - expected = np.array( - [ - [1.0, 1.8, -1.2, 0.0, -1.0, 0.0], - [1.89, 0.0, 0.0, 1.0, 0.0, 1.0], - [0.0, -1.74, 1.74, 1.0, 1.1, 1.0], - [0.0, 0.0, 0.0, -1.0, 0.0, -1.0], - [0.0, 1.0, -1.0, 0.0, -1.0, 0.0], - [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], - ] - ) - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_python_move_group" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupTest) diff --git a/moveit_ros/planning_interface/test/python_move_group.test b/moveit_ros/planning_interface/test/python_move_group.test deleted file mode 100644 index 4367da03ed..0000000000 --- a/moveit_ros/planning_interface/test/python_move_group.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - diff --git a/moveit_ros/planning_interface/test/python_move_group_ns.py b/moveit_ros/planning_interface/test/python_move_group_ns.py deleted file mode 100755 index 0f2be65b59..0000000000 --- a/moveit_ros/planning_interface/test/python_move_group_ns.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: William Baker -# -# This test is used to ensure planning with a MoveGroupInterface is -# possible if the robot's move_group node is in a different namespace - -import unittest -import numpy as np -import rospy -import rostest -import os - -from moveit_ros_planning_interface._moveit_move_group_interface import ( - MoveGroupInterface, -) -from moveit_msgs.msg import MoveItErrorCodes - - -class PythonMoveGroupNsTest(unittest.TestCase): - PLANNING_GROUP = "manipulator" - PLANNING_NS = "test_ns/" - - @classmethod - def setUpClass(self): - self.group = MoveGroupInterface( - self.PLANNING_GROUP, - "%srobot_description" % self.PLANNING_NS, - self.PLANNING_NS, - ) - - @classmethod - def tearDown(self): - pass - - def check_target_setting(self, expect, *args): - if len(args) == 0: - args = [expect] - self.group.set_joint_value_target(*args) - res = self.group.get_joint_value_target() - self.assertTrue( - np.all(np.asarray(res) == np.asarray(expect)), - "Setting failed for %s, values: %s" % (type(args[0]), res), - ) - - def test_target_setting(self): - n = self.group.get_variable_count() - self.check_target_setting([0.1] * n) - self.check_target_setting((0.2,) * n) - self.check_target_setting(np.zeros(n)) - self.check_target_setting( - [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()} - ) - self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) - - def plan(self, target): - self.group.set_joint_value_target(target) - return self.group.plan() - - def test_validation(self): - current = np.asarray(self.group.get_current_joint_values()) - - error_code1, plan1, time = self.plan(current + 0.2) - error_code2, plan2, time = self.plan(current + 0.2) - - # both plans should have succeeded: - error_code = MoveItErrorCodes() - error_code.deserialize(error_code1) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code2) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - - # first plan should execute - self.assertTrue(self.group.execute(plan1)) - - # second plan should be invalid now (due to modified start point) and rejected - self.assertFalse(self.group.execute(plan2)) - - # newly planned trajectory should execute again - error_code3, plan3, time = self.plan(current) - self.assertTrue(self.group.execute(plan3)) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code3) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_python_move_group" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupNsTest) diff --git a/moveit_ros/planning_interface/test/python_move_group_ns.test b/moveit_ros/planning_interface/test/python_move_group_ns.test deleted file mode 100644 index 082779ac94..0000000000 --- a/moveit_ros/planning_interface/test/python_move_group_ns.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - diff --git a/moveit_ros/planning_interface/test/robot_state_update.py b/moveit_ros/planning_interface/test/robot_state_update.py deleted file mode 100755 index cfd40858f9..0000000000 --- a/moveit_ros/planning_interface/test/robot_state_update.py +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env python - -import unittest -import numpy as np -import rospy -import rostest -import os - -from moveit_ros_planning_interface._moveit_move_group_interface import ( - MoveGroupInterface, -) -from moveit_msgs.msg import MoveItErrorCodes - - -class RobotStateUpdateTest(unittest.TestCase): - PLANNING_GROUP = "manipulator" - - @classmethod - def setUpClass(self): - self.group = MoveGroupInterface( - self.PLANNING_GROUP, "robot_description", rospy.get_namespace() - ) - - @classmethod - def tearDown(self): - pass - - def plan(self, target): - self.group.set_joint_value_target(target) - return self.group.plan() - - def test(self): - current = np.asarray(self.group.get_current_joint_values()) - for i in range(30): - target = current + np.random.uniform(-0.5, 0.5, size=current.shape) - # if plan was successfully executed, current state should be reported at target - error_code1, plan, time = self.plan(target) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code1) - if (error_code.val == MoveItErrorCodes.SUCCESS) and self.group.execute( - plan - ): - actual = np.asarray(self.group.get_current_joint_values()) - self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0)) - # otherwise current state should be still the same - else: - actual = np.asarray(self.group.get_current_joint_values()) - self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0)) - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_robot_state_update" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, RobotStateUpdateTest) - - # suppress cleanup segfault in ROS < Kinetic - os._exit(0) diff --git a/moveit_ros/planning_interface/test/robot_state_update.test b/moveit_ros/planning_interface/test/robot_state_update.test deleted file mode 100644 index f7849d7d7c..0000000000 --- a/moveit_ros/planning_interface/test/robot_state_update.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/moveit_ros/planning_interface/test/serialize_msg.py b/moveit_ros/planning_interface/test/serialize_msg.py deleted file mode 100644 index 469e68d7f9..0000000000 --- a/moveit_ros/planning_interface/test/serialize_msg.py +++ /dev/null @@ -1,116 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2020, RWTH Aachen University -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of RWTH Aachen University nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Bjarne von Horn -# - -from moveit_ros_planning_interface._moveit_planning_interface_test_serialize_msg_cpp_helper import ( - ByteStringTestHelper, -) -from geometry_msgs.msg import Vector3 -import unittest - -try: - # Try Python 2.7 behaviour first - from StringIO import StringIO - - py_version_maj = 2 -except ImportError: - # Use Python 3.x behaviour as fallback and choose the non-unicode version - from io import BytesIO as StringIO - - py_version_maj = 3 - - -class PythonMsgSerializeTest(unittest.TestCase): - def setUp(self): - self.helper = ByteStringTestHelper() - - def test_EmbeddedZeros(self): - self.assertTrue(self.helper.compareEmbeddedZeros(b"\xff\xef\x00\x10")) - - def test_ByteStringFromPchar(self): - # ByteString(const char*) constructor - self.assertEqual(self.helper.getBytesPChar(), b"abcdef") - - def test_ByteStringFromStdString(self): - # ByteString(const std::string&) constructor - self.assertEqual(self.helper.getBytesStdString(), b"\xff\xfe\x10\x00\x00") - - def test_ByteStringDefaultCtor(self): - self.assertEqual(self.helper.getDefaultBytes(), b"") - - def test_CxxTupleToPy(self): - # sending a tuple from C++ to Python - ans = self.helper.getTuple() - self.assertIsInstance(ans, tuple) - self.assertEqual(len(ans), 1) - self.assertEqual(ans[0], b"abcdef") - - def test_PyTupleToCxx(self): - # sending a tuple from Python to C++ - self.assertTrue(self.helper.compareTuple((b"mno",))) - - def test_sendMessage(self): - tmp = StringIO() - Vector3(x=1.0, y=-2.0, z=0.25).serialize(tmp) - self.assertTrue(self.helper.compareVector(tmp.getvalue())) - - def test_recieveMessage(self): - tmp = Vector3() - tmp.deserialize(self.helper.getVector()) - self.assertEqual(tmp, Vector3(1.0, -2.0, 0.25)) - - def test_rejectInt(self): - with self.assertRaisesRegexp(Exception, "Python argument types in"): - self.helper.compareEmbeddedZeros(4711) - - def test_rejectIntTuple(self): - with self.assertRaisesRegexp(Exception, "Python argument types in"): - self.helper.compareEmbeddedZeros((4711,)) - - def test_rejectUnicode(self): - with self.assertRaisesRegexp(Exception, "Python argument types in"): - self.helper.compareEmbeddedZeros(u"kdasd") # fmt: skip - - @unittest.skipIf(py_version_maj == 2, "does not trigger with python 2.7") - def test_rejectUnicodeTuple(self): - with self.assertRaisesRegexp( - RuntimeError, "Underlying python object is not a Bytes/String instance" - ): - self.helper.compareVectorTuple((u"kdasd",)) # fmt: skip - - -if __name__ == "__main__": - unittest.main() diff --git a/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp b/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp deleted file mode 100644 index 13c6e446d6..0000000000 --- a/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, RWTH Aachen University. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of RWTH Aachen University nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Bjarne von Horn */ - -#include -#include -#include -#include -#include - -namespace bp = boost::python; -using moveit::py_bindings_tools::ByteString; - -// Helper class to be exposed to Python -class ByteStringTestHelper -{ - // Helper to test whether a vector of unsigned chars has the same content as a bytes Object - bool doCompare(const std::vector& data, PyObject* obj) - { - const char* py_data = PyBytes_AsString(obj); - if (!py_data) - return false; - Py_ssize_t size = PyBytes_GET_SIZE(obj); - if (size < 0 || std::vector::size_type(size) != data.size()) - return false; - return std::memcmp(py_data, &data[0], size) == 0; - } - -public: - bool compareEmbeddedZeros(const ByteString& s) - { - const std::vector testdata{ 0xff, 0xef, 0x00, 0x10 }; - return doCompare(testdata, s.ptr()); - } - bool compareTuple(const bp::tuple& t) - { - const ByteString s(t[0]); - const std::vector testdata{ 'm', 'n', 'o' }; - return doCompare(testdata, s.ptr()); - } - - ByteString getBytesPChar() - { - return ByteString("abcdef"); - } - ByteString getBytesStdString() - { - std::string s; - s.push_back('\xff'); - s.push_back('\xfe'); - s.push_back('\x10'); - s.push_back('\x00'); - s.push_back('\x00'); - return ByteString(s); - } - ByteString getDefaultBytes() - { - return ByteString(); - } - bp::tuple getTuple() - { - return bp::make_tuple(ByteString("abcdef")); - } - ByteString getVector() - { - geometry_msgs::Vector3 v; - v.x = 1.0; - v.y = -2.0; - v.z = 0.25; - return ByteString(v); - } - bool compareVector(const ByteString& s) - { - geometry_msgs::Vector3 v; - s.deserialize(v); - return v.x == 1.0 && v.y == -2.0 && v.z == 0.25; - } - bool compareVectorTuple(const bp::tuple& t) - { - const ByteString s(t[0]); - return compareVector(s); - } - - static void setup() - { - bp::class_ cls("ByteStringTestHelper"); - cls.def("compareEmbeddedZeros", &ByteStringTestHelper::compareEmbeddedZeros); - cls.def("compareTuple", &ByteStringTestHelper::compareTuple); - cls.def("compareVectorTuple", &ByteStringTestHelper::compareVectorTuple); - cls.def("getBytesPChar", &ByteStringTestHelper::getBytesPChar); - cls.def("getBytesStdString", &ByteStringTestHelper::getBytesStdString); - cls.def("getDefaultBytes", &ByteStringTestHelper::getDefaultBytes); - cls.def("getTuple", &ByteStringTestHelper::getTuple); - cls.def("getVector", &ByteStringTestHelper::getVector); - cls.def("compareVector", &ByteStringTestHelper::compareVector); - } -}; - -BOOST_PYTHON_MODULE(_moveit_planning_interface_test_serialize_msg_cpp_helper) -{ - ByteStringTestHelper::setup(); -} diff --git a/moveit_ros/planning_interface/test/test_cleanup.py b/moveit_ros/planning_interface/test/test_cleanup.py deleted file mode 100755 index bb0aad1f72..0000000000 --- a/moveit_ros/planning_interface/test/test_cleanup.py +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import rospy -from moveit_ros_planning_interface._moveit_move_group_interface import ( - MoveGroupInterface, -) - -group = MoveGroupInterface("manipulator", "robot_description", rospy.get_namespace())