From 4598f6602ded91d91fccabe7a8bcc490a528fc54 Mon Sep 17 00:00:00 2001 From: Will Baker Date: Thu, 12 Apr 2018 16:04:39 -0500 Subject: [PATCH] Add namespace capabilities to moveit_commander (#835) * Add python goodies * Update planning interface tests for namespace arg * Add namespace tests * Update moveit commander for namespacing * Add moveit commander tests * Add movegroup test in namespace * Update param scopes in template and launch files * Code formatting * Add BSD License and name * Add description * Fix joy test --- moveit_commander/CMakeLists.txt | 2 + .../src/moveit_commander/move_group.py | 4 +- .../src/moveit_commander/robot.py | 7 +- moveit_commander/test/CMakeLists.txt | 15 ++ .../test/python_moveit_commander.py | 102 ++++++++++++ .../test/python_moveit_commander.test | 5 + .../test/python_moveit_commander_ns.py | 106 +++++++++++++ .../test/python_moveit_commander_ns.test | 5 + .../test/rrbot_move_group.launch | 4 +- .../test_cancel_before_plan_execution.test | 4 +- .../src/wrap_python_move_group.cpp | 72 +++++---- .../planning_scene_interface.h | 2 +- .../src/planning_scene_interface.cpp | 7 +- .../wrap_python_planning_scene_interface.cpp | 13 +- .../src/wrap_python_robot_interface.cpp | 149 +++++++++++++++++- .../planning_interface/test/CMakeLists.txt | 5 +- .../test/movegroup_interface.py | 3 +- .../test/python_move_group.py | 2 +- .../test/python_move_group_ns.py | 105 ++++++++++++ .../test/python_move_group_ns.test | 9 ++ .../test/robot_state_update.py | 2 +- .../moveitjoy_module.py | 2 +- .../launch/demo.launch | 4 +- 23 files changed, 570 insertions(+), 59 deletions(-) create mode 100644 moveit_commander/test/CMakeLists.txt create mode 100755 moveit_commander/test/python_moveit_commander.py create mode 100644 moveit_commander/test/python_moveit_commander.test create mode 100755 moveit_commander/test/python_moveit_commander_ns.py create mode 100644 moveit_commander/test/python_moveit_commander_ns.test create mode 100755 moveit_ros/planning_interface/test/python_move_group_ns.py create mode 100644 moveit_ros/planning_interface/test/python_move_group_ns.test diff --git a/moveit_commander/CMakeLists.txt b/moveit_commander/CMakeLists.txt index dffbe98a99..70ba103732 100644 --- a/moveit_commander/CMakeLists.txt +++ b/moveit_commander/CMakeLists.txt @@ -9,3 +9,5 @@ catkin_package() install(PROGRAMS bin/${PROJECT_NAME}_cmdline.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +add_subdirectory(test) diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index 6125b85c50..db3b01a7fb 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -47,9 +47,9 @@ class MoveGroupCommander(object): Execution of simple commands for a particular group """ - def __init__(self, name, robot_description="robot_description"): + def __init__(self, name, robot_description="robot_description", ns=""): """ Specify the group name for which to construct this commander instance. Throws an exception if there is an initialization error. """ - self._g = _moveit_move_group_interface.MoveGroupInterface(name, robot_description) + self._g = _moveit_move_group_interface.MoveGroupInterface(name, robot_description, ns) def get_name(self): """ Get the name of the group this instance was initialized for """ diff --git a/moveit_commander/src/moveit_commander/robot.py b/moveit_commander/src/moveit_commander/robot.py index 9d9390cae4..f9e5a64791 100644 --- a/moveit_commander/src/moveit_commander/robot.py +++ b/moveit_commander/src/moveit_commander/robot.py @@ -144,9 +144,10 @@ def pose(self): """ return conversions.list_to_pose_stamped(self._robot._r.get_link_pose(self._name), self._robot.get_planning_frame()) - def __init__(self, robot_description="robot_description"): + def __init__(self, robot_description="robot_description", ns=""): self._robot_description = robot_description - self._r = _moveit_robot_interface.RobotInterface(robot_description) + self._ns = ns + self._r = _moveit_robot_interface.RobotInterface(robot_description, ns) self._groups = {} self._joint_owner_groups = {} @@ -236,7 +237,7 @@ def get_group(self, name): if not self._groups.has_key(name): if not self.has_group(name): raise MoveItCommanderException("There is no group named %s" % name) - self._groups[name] = MoveGroupCommander(name, self._robot_description) + self._groups[name] = MoveGroupCommander(name, self._robot_description, self._ns) return self._groups[name] def has_group(self, name): diff --git a/moveit_commander/test/CMakeLists.txt b/moveit_commander/test/CMakeLists.txt new file mode 100644 index 0000000000..4f897ccbdf --- /dev/null +++ b/moveit_commander/test/CMakeLists.txt @@ -0,0 +1,15 @@ +if (CATKIN_ENABLE_TESTING) + find_package(moveit_resources) + find_package(rostest) + + add_rostest(python_moveit_commander.test) + add_rostest(python_moveit_commander_ns.test) +endif() + +install(PROGRAMS python_moveit_commander.py + python_moveit_commander_ns.py + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test) + +install(FILES python_moveit_commander.test + python_moveit_commander_ns.test + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test) diff --git a/moveit_commander/test/python_moveit_commander.py b/moveit_commander/test/python_moveit_commander.py new file mode 100755 index 0000000000..727d33389d --- /dev/null +++ b/moveit_commander/test/python_moveit_commander.py @@ -0,0 +1,102 @@ +#!/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 + +import unittest +import numpy as np +import rospy +import rostest +import os + +from moveit_commander import RobotCommander + + +class PythonMoveitCommanderTest(unittest.TestCase): + PLANNING_GROUP = "manipulator" + + @classmethod + def setUpClass(self): + self.commander = RobotCommander("robot_description") + self.group = self.commander.get_group(self.PLANNING_GROUP) + + @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()) + + plan1 = self.plan(current + 0.2) + plan2 = self.plan(current + 0.2) + + # 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 + plan3 = self.plan(current) + self.assertTrue(self.group.execute(plan3)) + + +if __name__ == '__main__': + PKGNAME = 'moveit_ros_planning_interface' + NODENAME = 'moveit_test_python_moveit_commander' + rospy.init_node(NODENAME) + rostest.rosrun(PKGNAME, NODENAME, PythonMoveitCommanderTest) + + # suppress cleanup segfault + os._exit(0) diff --git a/moveit_commander/test/python_moveit_commander.test b/moveit_commander/test/python_moveit_commander.test new file mode 100644 index 0000000000..c97a74198c --- /dev/null +++ b/moveit_commander/test/python_moveit_commander.test @@ -0,0 +1,5 @@ + + + + diff --git a/moveit_commander/test/python_moveit_commander_ns.py b/moveit_commander/test/python_moveit_commander_ns.py new file mode 100755 index 0000000000..efaa6f03d3 --- /dev/null +++ b/moveit_commander/test/python_moveit_commander_ns.py @@ -0,0 +1,106 @@ +#!/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 RobotCommander is +# possbile 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_commander import RobotCommander + + +class PythonMoveitCommanderNsTest(unittest.TestCase): + PLANNING_GROUP = "manipulator" + PLANNING_NS = "test_ns/" + + @classmethod + def setUpClass(self): + self.commander = RobotCommander("%srobot_description"%self.PLANNING_NS, self.PLANNING_NS) + self.group = self.commander.get_group(self.PLANNING_GROUP) + + @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()) + + plan1 = self.plan(current + 0.2) + plan2 = self.plan(current + 0.2) + + # 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 + plan3 = self.plan(current) + self.assertTrue(self.group.execute(plan3)) + + +if __name__ == '__main__': + PKGNAME = 'moveit_ros_planning_interface' + NODENAME = 'moveit_test_python_moveit_commander_ns' + rospy.init_node(NODENAME) + rostest.rosrun(PKGNAME, NODENAME, PythonMoveitCommanderNsTest) + + # suppress cleanup segfault + os._exit(0) diff --git a/moveit_commander/test/python_moveit_commander_ns.test b/moveit_commander/test/python_moveit_commander_ns.test new file mode 100644 index 0000000000..feda452a7e --- /dev/null +++ b/moveit_commander/test/python_moveit_commander_ns.test @@ -0,0 +1,5 @@ + + + + diff --git a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch index 10d6069e3f..f8351ec4f6 100644 --- a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch +++ b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch @@ -18,8 +18,8 @@ - - [/move_group/fake_controller_joint_states] + + [move_group/fake_controller_joint_states] diff --git a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test index b16f2736cf..edf4699d93 100644 --- a/moveit_ros/move_group/test/test_cancel_before_plan_execution.test +++ b/moveit_ros/move_group/test/test_cancel_before_plan_execution.test @@ -6,8 +6,8 @@ - - [/move_group/fake_controller_joint_states] + + [move_group/fake_controller_joint_states] 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 index 8b951b7d9a..22b24499af 100644 --- 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 @@ -61,10 +61,11 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer public: // ROSInitializer is constructed first, and ensures ros::init() was called, if // needed - MoveGroupInterfaceWrapper(const std::string& group_name, const std::string& robot_description) + MoveGroupInterfaceWrapper(const std::string& group_name, const std::string& robot_description, + const std::string& ns = "") : py_bindings_tools::ROScppInitializer() - , MoveGroupInterface(Options(group_name, robot_description), boost::shared_ptr(), - ros::WallDuration(5, 0)) + , MoveGroupInterface(Options(group_name, robot_description, ros::NodeHandle(ns)), + boost::shared_ptr(), ros::WallDuration(5, 0)) { } @@ -109,6 +110,15 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return setJointValueTarget(js_msg); } + bool setStateValueTarget(const std::string& state_str) + { + moveit_msgs::RobotState msg; + py_bindings_tools::deserializeMsg(state_str, msg); + robot_state::RobotState state(moveit::planning_interface::MoveGroupInterface::getJointValueTarget()); + moveit::core::robotStateMsgToRobotState(msg, state); + return moveit::planning_interface::MoveGroupInterface::setJointValueTarget(state); + } + bp::list getJointValueTargetPythonList() { const robot_state::RobotState& values = moveit::planning_interface::MoveGroupInterface::getJointValueTarget(); @@ -118,6 +128,14 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return l; } + std::string getJointValueTarget() + { + moveit_msgs::RobotState msg; + const robot_state::RobotState state = moveit::planning_interface::MoveGroupInterface::getJointValueTarget(); + 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)); @@ -128,6 +146,13 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return getPlanningFrame().c_str(); } + std::string getInterfaceDescriptionPython() + { + moveit_msgs::PlannerInterfaceDescription msg; + getInterfaceDescription(msg); + return py_bindings_tools::serializeMsg(msg); + } + bp::list getActiveJointsList() const { return py_bindings_tools::listFromString(getActiveJoints()); @@ -303,6 +328,11 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer convertListToArrayOfPoses(poses, msg); return setPoseTargets(msg, end_effector_link); } + std::string 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 = "") { @@ -349,7 +379,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer std::map positions = getNamedTargetValues(name); std::map::iterator iterator; - for (iterator = positions.begin(); iterator != positions.end(); iterator++) + for (iterator = positions.begin(); iterator != positions.end(); ++iterator) output[iterator->first] = iterator->second; return output; } @@ -388,11 +418,6 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return asyncExecute(plan) == MoveItErrorCode::SUCCESS; } - const char* getPlannerIdCStr() const - { - return getPlannerId().c_str(); - } - std::string getPlanPython() { MoveGroupInterface::Plan plan; @@ -456,20 +481,6 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return constraints_str; } - void setTrajectoryConstraintsFromMsg(const std::string& constraints_str) - { - moveit_msgs::TrajectoryConstraints constraints_msg; - py_bindings_tools::deserializeMsg(constraints_str, constraints_msg); - setTrajectoryConstraints(constraints_msg); - } - - std::string getTrajectoryConstraintsPython() - { - moveit_msgs::TrajectoryConstraints constraints_msg(getTrajectoryConstraints()); - std::string constraints_str = py_bindings_tools::serializeMsg(constraints_msg); - return constraints_str; - } - std::string retimeTrajectory(const std::string& ref_state_str, const std::string& traj_str, double velocity_scaling_factor) { @@ -507,8 +518,8 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer class MoveGroupWrapper : public MoveGroupInterfaceWrapper { public: - MoveGroupWrapper(const std::string& group_name, const std::string& robot_description) - : MoveGroupInterfaceWrapper(group_name, robot_description) + MoveGroupWrapper(const std::string& group_name, const std::string& robot_description, const std::string& ns = "") + : MoveGroupInterfaceWrapper(group_name, robot_description, ns) { ROS_WARN("The MoveGroup class is deprecated and will be removed in ROS lunar. Please use MoveGroupInterface " "instead."); @@ -518,7 +529,7 @@ class MoveGroupWrapper : public MoveGroupInterfaceWrapper static void wrap_move_group_interface() { bp::class_ MoveGroupInterfaceClass( - "MoveGroupInterface", bp::init()); + "MoveGroupInterface", bp::init()); MoveGroupInterfaceClass.def("async_move", &MoveGroupInterfaceWrapper::asyncMovePython); MoveGroupInterfaceClass.def("move", &MoveGroupInterfaceWrapper::movePython); @@ -536,6 +547,7 @@ static void wrap_move_group_interface() MoveGroupInterfaceClass.def("get_name", &MoveGroupInterfaceWrapper::getNameCStr); MoveGroupInterfaceClass.def("get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr); + MoveGroupInterfaceClass.def("get_interface_description", &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython); MoveGroupInterfaceClass.def("get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList); MoveGroupInterfaceClass.def("get_joints", &MoveGroupInterfaceWrapper::getJointsList); @@ -574,6 +586,7 @@ static void wrap_move_group_interface() bool (MoveGroupInterfaceWrapper::*setJointValueTarget_4)(const std::string&, double) = &MoveGroupInterfaceWrapper::setJointValueTarget; MoveGroupInterfaceClass.def("set_joint_value_target", setJointValueTarget_4); + MoveGroupInterfaceClass.def("set_state_value_target", &MoveGroupInterfaceWrapper::setStateValueTarget); MoveGroupInterfaceClass.def("set_joint_value_target_from_pose", &MoveGroupInterfaceWrapper::setJointValueTargetFromPosePython); @@ -622,12 +635,6 @@ static void wrap_move_group_interface() MoveGroupInterfaceClass.def("set_path_constraints_from_msg", &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg); MoveGroupInterfaceClass.def("get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython); MoveGroupInterfaceClass.def("clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints); - - MoveGroupInterfaceClass.def("set_trajectory_constraints_from_msg", - &MoveGroupInterfaceWrapper::setTrajectoryConstraintsFromMsg); - MoveGroupInterfaceClass.def("get_trajectory_constraints", &MoveGroupInterfaceWrapper::getTrajectoryConstraintsPython); - MoveGroupInterfaceClass.def("clear_trajectory_constraints", &MoveGroupInterfaceWrapper::clearTrajectoryConstraints); - MoveGroupInterfaceClass.def("get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList); MoveGroupInterfaceClass.def("set_constraints_database", &MoveGroupInterfaceWrapper::setConstraintsDatabase); MoveGroupInterfaceClass.def("set_workspace", &MoveGroupInterfaceWrapper::setWorkspace); @@ -638,7 +645,6 @@ static void wrap_move_group_interface() MoveGroupInterfaceClass.def("set_max_acceleration_scaling_factor", &MoveGroupWrapper::setMaxAccelerationScalingFactor); MoveGroupInterfaceClass.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId); - MoveGroupInterfaceClass.def("get_planner_id", &MoveGroupInterfaceWrapper::getPlannerIdCStr); MoveGroupInterfaceClass.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts); MoveGroupInterfaceClass.def("compute_plan", &MoveGroupInterfaceWrapper::getPlanPython); MoveGroupInterfaceClass.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython); diff --git a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index 856d872f8e..d421b5484a 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -53,7 +53,7 @@ MOVEIT_CLASS_FORWARD(PlanningSceneInterface); class PlanningSceneInterface { public: - PlanningSceneInterface(); + explicit PlanningSceneInterface(const std::string& ns = ""); ~PlanningSceneInterface(); /** diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index 02d12cde23..9bdcb7948b 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -48,8 +48,9 @@ namespace planning_interface class PlanningSceneInterface::PlanningSceneInterfaceImpl { public: - PlanningSceneInterfaceImpl() + explicit PlanningSceneInterfaceImpl(const std::string& ns = "") { + node_handle_ = ros::NodeHandle(ns); planning_scene_service_ = node_handle_.serviceClient(move_group::GET_PLANNING_SCENE_SERVICE_NAME); apply_planning_scene_service_ = @@ -268,9 +269,9 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl robot_model::RobotModelConstPtr robot_model_; }; -PlanningSceneInterface::PlanningSceneInterface() +PlanningSceneInterface::PlanningSceneInterface(const std::string& ns) { - impl_ = new PlanningSceneInterfaceImpl(); + impl_ = new PlanningSceneInterfaceImpl(ns); } PlanningSceneInterface::~PlanningSceneInterface() 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 index 16920669cd..9725f0df22 100644 --- 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 @@ -55,7 +55,8 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial { public: // ROSInitializer is constructed first, and ensures ros::init() was called, if needed - PlanningSceneInterfaceWrapper() : py_bindings_tools::ROScppInitializer(), PlanningSceneInterface() + PlanningSceneInterfaceWrapper(const std::string& ns) + : py_bindings_tools::ROScppInitializer(), PlanningSceneInterface(ns) { } @@ -102,11 +103,18 @@ class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitial return py_bindings_tools::dictFromType(ser_aobjs); } + + bool applyPlanningScenePython(const std::string& ps_str) + { + moveit_msgs::PlanningScene ps_msg; + py_bindings_tools::deserializeMsg(ps_str, ps_msg); + return applyPlanningScene(ps_msg); + } }; static void wrap_planning_scene_interface() { - bp::class_ PlanningSceneClass("PlanningSceneInterface"); + bp::class_ PlanningSceneClass("PlanningSceneInterface", bp::init()); PlanningSceneClass.def("get_known_object_names", &PlanningSceneInterfaceWrapper::getKnownObjectNamesPython); PlanningSceneClass.def("get_known_object_names_in_roi", @@ -114,6 +122,7 @@ static void wrap_planning_scene_interface() PlanningSceneClass.def("get_object_poses", &PlanningSceneInterfaceWrapper::getObjectPosesPython); PlanningSceneClass.def("get_objects", &PlanningSceneInterfaceWrapper::getObjectsPython); PlanningSceneClass.def("get_attached_objects", &PlanningSceneInterfaceWrapper::getAttachedObjectsPython); + PlanningSceneClass.def("apply_planning_scene", &PlanningSceneInterfaceWrapper::applyPlanningScenePython); } } } 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 index aae8311a24..a5450a74cc 100644 --- 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 @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -54,12 +55,14 @@ namespace moveit class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer { public: - RobotInterfacePython(const std::string& robot_description) : py_bindings_tools::ROScppInitializer() + 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()); + current_state_monitor_ = + planning_interface::getSharedStateMonitor(robot_model_, planning_interface::getSharedTF(), ns); } const char* getRobotName() const @@ -160,6 +163,20 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return l; } + bp::list getDefaultStateNames(const std::string& group) + { + bp::list l; + const robot_model::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; @@ -178,6 +195,16 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return l; } + bp::dict getJointValues(const std::string& group, const std::string& named_state) + { + const robot_model::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_) @@ -206,6 +233,111 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer return py_bindings_tools::serializeMsg(msg); } + bp::tuple getEndEffectorParentGroup(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 robot_state::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); + } + + std::string getRobotMarkersPythonDictList(bp::dict& values, bp::list& links) + { + robot_state::RobotStatePtr state; + if (ensureCurrentState()) + { + state = current_state_monitor_->getCurrentState(); + } + else + { + state.reset(new robot_state::RobotState(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); + } + + std::string getRobotMarkersPythonDict(bp::dict& values) + { + bp::list links = py_bindings_tools::listFromString(robot_model_->getLinkModelNames()); + return getRobotMarkersPythonDictList(values, links); + } + + std::string getRobotMarkersFromMsg(const std::string& state_str) + { + moveit_msgs::RobotState state_msg; + robot_state::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); + } + + std::string getRobotMarkers() + { + if (!ensureCurrentState()) + return ""; + robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); + visualization_msgs::MarkerArray msg; + s->getRobotMarkers(msg, s->getRobotModel()->getLinkModelNames()); + + return py_bindings_tools::serializeMsg(msg); + } + + std::string getRobotMarkersPythonList(bp::list links) + { + if (!ensureCurrentState()) + return ""; + robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); + visualization_msgs::MarkerArray msg; + s->getRobotMarkers(msg, py_bindings_tools::stringFromList(links)); + + return py_bindings_tools::serializeMsg(msg); + } + + std::string getRobotMarkersGroup(std::string group) + { + if (!ensureCurrentState()) + return ""; + robot_state::RobotStatePtr s = current_state_monitor_->getCurrentState(); + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + visualization_msgs::MarkerArray msg; + if (jmg) + { + s->getRobotMarkers(msg, jmg->getLinkModelNames()); + } + + return py_bindings_tools::serializeMsg(msg); + } + + std::string getRobotMarkersGroupPythonDict(std::string group, bp::dict& values) + { + const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); + if (!jmg) + return ""; + bp::list links = py_bindings_tools::listFromString(jmg->getLinkModelNames()); + return getRobotMarkersPythonDictList(values, links); + } + bp::dict getCurrentVariableValues() { bp::dict d; @@ -233,6 +365,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer private: robot_model::RobotModelConstPtr robot_model_; planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; + ros::NodeHandle nh_; }; } @@ -240,10 +373,11 @@ static void wrap_robot_interface() { using namespace moveit; - bp::class_ RobotClass("RobotInterface", bp::init()); + bp::class_ RobotClass("RobotInterface", bp::init()); RobotClass.def("get_joint_names", &RobotInterfacePython::getJointNames); RobotClass.def("get_group_joint_names", &RobotInterfacePython::getGroupJointNames); + RobotClass.def("get_group_default_states", &RobotInterfacePython::getDefaultStateNames); RobotClass.def("get_group_joint_tips", &RobotInterfacePython::getGroupJointTips); RobotClass.def("get_group_names", &RobotInterfacePython::getGroupNames); RobotClass.def("get_link_names", &RobotInterfacePython::getLinkNames); @@ -254,9 +388,18 @@ static void wrap_robot_interface() RobotClass.def("get_current_state", &RobotInterfacePython::getCurrentState); RobotClass.def("get_current_variable_values", &RobotInterfacePython::getCurrentVariableValues); RobotClass.def("get_current_joint_values", &RobotInterfacePython::getCurrentJointValues); + RobotClass.def("get_joint_values", &RobotInterfacePython::getJointValues); RobotClass.def("get_robot_root_link", &RobotInterfacePython::getRobotRootLink); RobotClass.def("has_group", &RobotInterfacePython::hasGroup); RobotClass.def("get_robot_name", &RobotInterfacePython::getRobotName); + RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkers); + RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonList); + RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersFromMsg); + RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDictList); + RobotClass.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDict); + RobotClass.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroup); + RobotClass.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroupPythonDict); + RobotClass.def("get_parent_group", &RobotInterfacePython::getEndEffectorParentGroup); } BOOST_PYTHON_MODULE(_moveit_robot_interface) diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 11156bef5d..15b98c248a 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -6,8 +6,9 @@ if (CATKIN_ENABLE_TESTING) target_link_libraries(test_cleanup moveit_move_group_interface) add_rostest(python_move_group.test) + add_rostest(python_move_group_ns.test) add_rostest(robot_state_update.test) endif() -install(PROGRAMS python_move_group.py DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test) -install(FILES python_move_group.test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test) +install(PROGRAMS python_move_group.py python_move_group_ns.py DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test) +install(FILES python_move_group.test python_move_group_ns.test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test) diff --git a/moveit_ros/planning_interface/test/movegroup_interface.py b/moveit_ros/planning_interface/test/movegroup_interface.py index 17d6af78dc..1567c1ea3c 100755 --- a/moveit_ros/planning_interface/test/movegroup_interface.py +++ b/moveit_ros/planning_interface/test/movegroup_interface.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +import rospy from moveit_ros_planning_interface._moveit_move_group_interface import MoveGroupInterface -group = MoveGroupInterface("manipulator", "robot_description") +group = MoveGroupInterface("manipulator", "robot_description", rospy.get_namespace()) diff --git a/moveit_ros/planning_interface/test/python_move_group.py b/moveit_ros/planning_interface/test/python_move_group.py index 8b23e396c1..acabe36eeb 100755 --- a/moveit_ros/planning_interface/test/python_move_group.py +++ b/moveit_ros/planning_interface/test/python_move_group.py @@ -14,7 +14,7 @@ class PythonMoveGroupTest(unittest.TestCase): @classmethod def setUpClass(self): - self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description") + self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace()) @classmethod def tearDown(self): diff --git a/moveit_ros/planning_interface/test/python_move_group_ns.py b/moveit_ros/planning_interface/test/python_move_group_ns.py new file mode 100755 index 0000000000..88efc06858 --- /dev/null +++ b/moveit_ros/planning_interface/test/python_move_group_ns.py @@ -0,0 +1,105 @@ +#!/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 +# possbile 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 + + +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.compute_plan() + + def test_validation(self): + current = np.asarray(self.group.get_current_joint_values()) + + plan1 = self.plan(current + 0.2) + plan2 = self.plan(current + 0.2) + + # 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 + plan3 = self.plan(current) + self.assertTrue(self.group.execute(plan3)) + + +if __name__ == '__main__': + PKGNAME = 'moveit_ros_planning_interface' + NODENAME = 'moveit_test_python_move_group' + rospy.init_node(NODENAME) + rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupNsTest) + + # suppress cleanup segfault + os._exit(0) diff --git a/moveit_ros/planning_interface/test/python_move_group_ns.test b/moveit_ros/planning_interface/test/python_move_group_ns.test new file mode 100644 index 0000000000..b9293ac105 --- /dev/null +++ b/moveit_ros/planning_interface/test/python_move_group_ns.test @@ -0,0 +1,9 @@ + + + + + + + diff --git a/moveit_ros/planning_interface/test/robot_state_update.py b/moveit_ros/planning_interface/test/robot_state_update.py index de98da22af..f85c5328e1 100755 --- a/moveit_ros/planning_interface/test/robot_state_update.py +++ b/moveit_ros/planning_interface/test/robot_state_update.py @@ -14,7 +14,7 @@ class RobotStateUpdateTest(unittest.TestCase): @classmethod def setUpClass(self): - self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description") + self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace()) @classmethod def tearDown(self): diff --git a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py index 8ab63e4ac3..be33fc7d15 100644 --- a/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py +++ b/moveit_ros/visualization/src/moveit_ros_visualization/moveitjoy_module.py @@ -348,7 +348,7 @@ def new(self, status, attr): class MoveitJoy: def parseSRDF(self): - ri = RobotInterface("/robot_description") + ri = RobotInterface("/robot_description", "") planning_groups = {} for g in ri.get_group_names(): self.planning_groups_tips[g] = ri.get_group_joint_tips(g) diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch index 84a3e669df..b4b80c38e2 100644 --- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch +++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/demo.launch @@ -28,8 +28,8 @@ - - [/move_group/fake_controller_joint_states] + + [move_group/fake_controller_joint_states]