[moveit] Add workaround for default solver issue of MoveIt! tork-a/rt…

…mros_nextage#170

hironx_ros_bridge/test/test_hironx_moveit.py

@@ -60,6 +60,15 @@ def setUpClass(self):
         self.larm = MoveGroupCommander("left_arm")
         self.botharms = MoveGroupCommander("both_arm")
 
+        # Tentative workaround for https://github.com/tork-a/rtmros_nextage/issues/170
+        safe_kinematicsolver = "RRTConnectkConfigDefault"
+        self.larm.set_planner_id(safe_kinematicsolver)
+        self.rarm.set_planner_id(safe_kinematicsolver)
+        self.bothrarms.set_planner_id(safe_kinematicsolver)
+
+        self.larm = MoveGroupCommander("left_arm")
+        self.botharms = MoveGroupCommander("both_arm")
+
         self._botharms_joints = ['LARM_JOINT0', 'LARM_JOINT1',
                                  'LARM_JOINT2', 'LARM_JOINT3',
                                  'LARM_JOINT4', 'LARM_JOINT5',
@@ -140,9 +149,15 @@ def test_botharms_set_named_target(self):
         '''
         # Move the arms to non init pose.
         self.larm.set_pose_target(self.banzai_pose_larm_goal)
+        self.larm.plan()
+        self.larm.go()
         self.rarm.set_pose_target(self.banzai_pose_rarm_goal)
+        self.rarm.plan()
+        self.rarm.go()
 
         self.botharms.set_named_target('init_rtm')
+        self.botharms.plan()
+        self.botharms.go()
         self.assertAlmostEqual(self.botharms.get_current_joint_values(), self.init_rtm_jointvals, places=3)