def send_goal_and_dont_wait(self,
goal=None,
goal_type=MoveGoal.PLAN_AND_EXECUTE,
stop_after=20):
if goal is None:
goal = MoveActionGoal()
goal.goal = self.wrapper._get_goal()
goal.goal.type = goal_type
i = 0
self.loop_once()
t1 = Thread(
target=self.get_as()._as.action_server.internal_goal_callback,
args=(goal, ))
self.loop_once()
t1.start()
sleeper = rospy.Rate(self.tick_rate)
while self.results.empty():
self.loop_once()
sleeper.sleep()
i += 1
if i > stop_after:
self.interrupt()
t1.join()
self.loop_once()
result = self.results.get()
return result
def send_goal(self,
goal=None,
goal_type=MoveGoal.PLAN_AND_EXECUTE,
wait=True):
"""
:rtype: MoveResult
"""
if goal is None:
goal = MoveActionGoal()
goal.goal = self.wrapper._get_goal()
goal.goal.type = goal_type
i = 0
self.loop_once()
t1 = Thread(
target=self.get_as()._as.action_server.internal_goal_callback,
args=(goal, ))
self.loop_once()
t1.start()
sleeper = rospy.Rate(self.tick_rate)
while self.results.empty():
self.loop_once()
sleeper.sleep()
i += 1
t1.join()
self.loop_once()
result = self.results.get()
return result
def test_empty_joint_goal2(self, zero_pose):
"""
:type zero_pose: Donbot
"""
# zero_pose.allow_self_collision()
goal = MoveActionGoal()
move_cmd = MoveCmd()
joint_goal1 = JointConstraint()
joint_goal1.type = JointConstraint.JOINT
joint_goal1.goal_state.name = [
u'ur5_shoulder_pan_joint', u'ur5_shoulder_lift_joint',
u'ur5_elbow_joint', u'ur5_wrist_1_joint', u'ur5_wrist_2_joint',
u'ur5_wrist_3_joint'
]
joint_goal1.goal_state.position = [
-0.15841275850404912, -2.2956998983966272, 2.240689277648926,
-2.608211342488424, -2.7356796900378626, -2.5249870459186
]
joint_goal2 = JointConstraint()
joint_goal2.type = JointConstraint.JOINT
move_cmd.joint_constraints.append(joint_goal1)
move_cmd.joint_constraints.append(joint_goal2)
goal.goal.cmd_seq.append(move_cmd)
goal.goal.type = goal.goal.PLAN_AND_EXECUTE
zero_pose.send_and_check_goal(goal=goal)
def send_goal(self):
"""
:rtype: MoveResult
"""
goal = MoveActionGoal()
goal.goal = self.wrapper._get_goal()
t1 = Thread(target=self.pm._plugins[u'action server']._as.
action_server.internal_goal_callback,
args=(goal, ))
t1.start()
while self.results.empty():
self.loop_once()
t1.join()
self.loop_once()
result = self.results.get()
return result
def test_empty_goal(self, zero_pose):
"""
:type zero_pose: Donbot
"""
zero_pose.allow_self_collision()
goal = MoveActionGoal()
goal.goal.type = MoveGoal.PLAN_AND_EXECUTE
result = zero_pose.send_goal(goal)
assert result.error_code == MoveResult.INSOLVABLE
def test_undefined_type(self, zero_pose):
"""
:type zero_pose: Donbot
"""
zero_pose.allow_self_collision()
goal = MoveActionGoal()
goal.goal.type = MoveGoal.UNDEFINED
result = zero_pose.send_goal(goal)
assert result.error_code == MoveResult.INSOLVABLE