def single_run(self, goal):
def done_cb(state, result):
assert state == GoalStatus.SUCCEEDED, "Kick was not successful"
self.kick_succeeded = True
self.kick_succeeded = False
sub = MockSubscriber('kick_motor_goals', JointCommand)
self.with_assertion_grace_period(
lambda: self.assertNodeRunning("dynamic_kick"), 20)
client = ActionClient(self, KickAction, 'dynamic_kick')
assert client.wait_for_server(), "Kick action server not running"
client.send_goal_async(goal)
client.done_cb = done_cb
client.wait_for_result()
sub.wait_until_connected()
sub.assertMessageReceived()
assert self.kick_succeeded, "Kick was not successful"
def one_run(self, direction):
def done_cb(state, result):
assert state == GoalStatus.SUCCEEDED, "Dynup was not successful"
self.kick_succeeded = True
self.kick_succeeded = False
sub = MockSubscriber('dynup_motor_goals', JointCommand)
self.with_assertion_grace_period(
lambda: self.assertNodeRunning("dynup"), 20)
client = ActionClient(self, DynUpAction, 'dynup')
assert client.wait_for_server(), "Dynup action server not running"
goal = DynUpGoal()
goal.direction = direction
client.send_goal_async(goal)
client.done_cb = done_cb
client.wait_for_result()
sub.wait_until_connected()
sub.assertMessageReceived()
assert self.kick_succeeded, "Dynup was not successful"
else:
print('Unknown state', state)
print(str(result))
rclpy.init(args=None)
self.get_logger().warn(
"Make sure that the Dynup is running together with simulator or this script will not work."
)
rospy.Subscriber("/DynamixelController/command", JointCommand, callback)
print('[..] Connecting to action server \'dynup\'', end='')
sys.stdout.flush()
client = ActionClient(self, DynUpAction, 'dynup')
if not client.wait_for_server():
exit(1)
print('\r[OK] Connecting to action server \'dynup\'')
print()
goal = DynUpGoal()
goal.direction = "rise"
client.send_goal_async(goal)
client.done_cb = done_cb
print("Sent new goal. Waiting for result")
client.wait_for_result()
self.get_logger().info(
"Your walkready animation has been saved to the current directory.")
