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"
class NavNode(Node):
def __init__(self):
""" Set up the node. """
super().__init__('nav_node')
self.ac = ActionClient(self, MoveBaseAction, "move_base")
def goto_point(self, x_target, y_target, theta_target=0):
""" Move to a location relative to the robot's current position """
self.get_logger().info("navigating to: ({},{})".format(
x_target, y_target))
goal = create_goal_message(x_target, y_target, theta_target,
'base_link')
self.get_logger().info("Waiting for server.")
self.ac.wait_for_server()
self.get_logger().info("Sending goal.")
self.ac.send_goal_async(goal, feedback_callback=self.feedback_callback)
self.get_logger().info("Goal Sent.")
# Wait until the server reports a result.
self.ac.wait_for_result()
self.get_logger().info("Status Text: {}".format(
self.ac.get_goal_status_text()))
# Should be either "SUCCEEDED" or "ABORTED"
state_name = actionlib.get_name_of_constant(GoalStatus,
self.ac.get_state())
self.get_logger().info("State : {}".format(state_name))
def feedback_callback(self, feedback_msg):
feedback = feedback_msg.feedback
self.get_logger().info("Received feedback: {0}".format(
feedback.partial_sequence))
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.")
