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

@@ -9,3 +9,5 @@ catkin_package()
 
 install(PROGRAMS bin/${PROJECT_NAME}_cmdline.py
         DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+add_subdirectory(test)

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 """

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):

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)

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)

moveit_commander/test/python_moveit_commander.test

@@ -0,0 +1,5 @@
+<launch>
+  <include file="$(find moveit_resources)/fanuc_moveit_config/launch/test_environment.launch"/>
+  <test pkg="moveit_commander" type="python_moveit_commander.py" test-name="python_moveit_commander"
+        time-limit="300" args=""/>
+</launch>

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)

moveit_commander/test/python_moveit_commander_ns.test

@@ -0,0 +1,5 @@
+<launch>
+  <include ns="test_ns" file="$(find moveit_resources)/fanuc_moveit_config/launch/test_environment.launch"/>
+  <test pkg="moveit_commander" type="python_moveit_commander_ns.py" test-name="python_moveit_commander_ns"
+        time-limit="300" args=""/>
+</launch>

moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch

@@ -18,8 +18,8 @@
 
   <!-- We do not have a robot connected, so publish fake joint states -->
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="/use_gui" value="$(arg use_gui)"/>
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
+    <param name="use_gui" value="$(arg use_gui)"/>
+    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
   </node>
 
   <!-- Given the published joint states, publish tf for the robot links -->

moveit_ros/move_group/test/test_cancel_before_plan_execution.test

@@ -6,8 +6,8 @@
 
 	<!-- We do not have a robot connected, so publish fake joint states -->
 	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-		<param name="/use_gui" value="false"/>
-		<rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
+		<param name="use_gui" value="false"/>
+		<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
 	</node>
 
 	<!-- Given the published joint states, publish tf for the robot links -->