def move_leg(self, num, x, y, z):
msg = MoveLeg.Goal()
msg.x = float(x)
msg.y = float(y)
msg.z = float(z)
self._action_clients[num].wait_for_server()
self._send_goal_future[num] = self._action_clients[num].send_goal_async(msg)
self._send_goal_future[num].add_done_callback(lambda future, num=num: self.goal_response_callback(future, num))
def move_leg(self, num, x, y, z, stop_by_z=False):
msg = MoveLeg.Goal()
msg.x = float(x)
msg.y = float(y)
msg.z = float(z)
msg.abort_if_forcesensor_z_detect_contact = stop_by_z
self._action_clients[num].wait_for_server()
self._send_goal_future[num] = self._action_clients[num].send_goal_async(msg)
self._send_goal_future[num].add_done_callback(lambda future, num=num: self.goal_response_callback(future, num))
def move_leg_default_position(self, num):
msg = MoveLeg.Goal()
msg.relative_mode = True
self._move_leg_action_clients[num].wait_for_server()
self._send_goal_future[num] = self._move_leg_action_clients[
num].send_goal_async(msg)
self._send_goal_future[num].add_done_callback(
lambda future, num=num: self.action_goal_response_callback(
future, num))
def move_leg_and_wait(self, x, y, z):
self._move_leg_action_client.wait_for_server()
goal_msg = MoveLeg.Goal()
goal_msg.x = float(x)
goal_msg.y = float(y)
goal_msg.z = float(z)
goal_msg.relative_mode = True
send_goal_future = self._move_leg_action_client.send_goal_async(goal_msg)
# rclpy.spin_until_future_complete(self, send_goal_future)
while not send_goal_future.done():
pass
move_leg_goal_handle = send_goal_future.result()
get_result_future = move_leg_goal_handle.get_result_async()
# rclpy.spin_until_future_complete(self, get_result_future)
while not get_result_future.done():
pass
def move_leg_and_wait(self, x, y, z, stop_condition=(lambda: False)):
self._move_leg_action_client.wait_for_server()
goal_msg = MoveLeg.Goal()
goal_msg.x = float(x)
goal_msg.y = float(y)
goal_msg.z = float(z + self._z_offset)
goal_msg.relative_mode = True
send_goal_future = self._move_leg_action_client.send_goal_async(
goal_msg)
# rclpy.spin_until_future_complete(self, send_goal_future)
while not send_goal_future.done():
time.sleep(0.01)
pass
self._move_leg_goal_handle = send_goal_future.result()
self._leg_stop_condition = stop_condition
self._leg_stoped = False
self._timer_start = time.time()
get_result_future = self._move_leg_goal_handle.get_result_async()
# rclpy.spin_until_future_complete(self, get_result_future)
while not get_result_future.done():
time.sleep(0.01)
pass
result = get_result_future.result().result
self._x_result = result.x
self._y_result = result.y
self._z_result = result.z
self.get_logger().debug('z={0}'.format(self._z_result))
self._move_leg_goal_handle = None
self._leg_stop_condition = (lambda: False)
return self._leg_stoped
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_action_client')
leg_name = 'front_left'
forcesensor_sub = node.create_subscription(ForceSensor, 'hapthexa/leg/'+leg_name+'/force_sensor', lambda msg: forcesensor_callback(msg, node),10)
action_client = ActionClient(node, MoveLeg, 'hapthexa/leg/'+leg_name+'/move_leg')
node.get_logger().info('Waiting for action server...')
action_client.wait_for_server()
global goal_handle
global phase
phase = 1
global once_failed
once_failed = False
goal_msg = MoveLeg.Goal()
goal_msg.x = float(0)
goal_msg.y = float(0)
goal_msg.z = float(10.0)
goal_msg.relative_mode = True
send_goal_future = action_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
#########################
phase = 2
goal_msg = MoveLeg.Goal()
goal_msg.x = float(5.0)
goal_msg.y = float(0)
goal_msg.z = float(10.0)
goal_msg.relative_mode = True
send_goal_future = action_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
#########################
if once_failed:
phase = 3
goal_msg = MoveLeg.Goal()
goal_msg.x = float(0.0)
goal_msg.y = float(0)
goal_msg.z = float(15.0)
goal_msg.relative_mode = True
send_goal_future = action_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
#########################
phase = 4
goal_msg = MoveLeg.Goal()
goal_msg.x = float(5.0)
goal_msg.y = float(0)
goal_msg.z = float(15.0)
goal_msg.relative_mode = True
send_goal_future = action_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
#########################
phase = 5
goal_msg = MoveLeg.Goal()
goal_msg.x = float(5.0)
goal_msg.y = float(0)
goal_msg.z = float(0.0)
goal_msg.relative_mode = True
send_goal_future = action_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(node, send_goal_future)
goal_handle = send_goal_future.result()
get_result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(node, get_result_future)
action_client.destroy()
node.destroy_node()
rclpy.shutdown()