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