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()