Calling the DoObjectiveSequence Action

danger

With great power comes great responsibility. If in your code you use our SDK to call DoObjectiveSequence and then lose the goal handle for the action, you will no longer be able to cancel the running Objective. If the Objective has no end condition (and runs indefinitely) you will either need to use the web UI to cancel the Objective, or restart MoveIt Pro.

Instead of calling the /execute_objective service, it is also possible to call the lower level ROS2 action /do_objective for a more granular approach.

Unfortunately, because of lack of support for ROS2 action in roslibpy, we must use the rclpy library instead.

The DoObjectiveSequence action is used to execute a MoveIt Pro Objective, but the user is responsible for handling the goal state and possible error codes. It is most useful and powerful for a custom ROS application to interface directly with MoveIt Pro's Objective server, allowing you to control exactly when to launch and cancel an Objective.

Example Usage

The following Python code snippet demonstrates how to call the DoObjectiveSequence action from an external application:

"""
Run an objective using a ROS2 action client.
"""

from typing import List, Optional, Any
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from rclpy.task import Future
from rclpy.action.client import ClientGoalHandle

from moveit_studio_sdk_msgs.action import DoObjectiveSequence

# Objective example.
objective_xml_string: str = """
<root BTCPP_format="4" main_tree_to_execute="simple_example">
  <BehaviorTree ID="simple_example">
    <Control ID="Sequence" name="TopLevelSequence">
      <Action ID="AlwaysSuccess"/>
    </Control>
  </BehaviorTree>
</root>
"""


def main(args: Optional[List[str]] = None) -> None:
    """Execute the objective."""
    rclpy.init(args=args)

    node: Node = Node("objective_client")
    action_client: ActionClient = ActionClient(
        node, DoObjectiveSequence, "/do_objective"
    )
    goal_handle: Optional[ClientGoalHandle] = None

    try:
        # Wait until the action server is available
        if not action_client.wait_for_server(timeout_sec=5.0):
            node.get_logger().error("Action server not available after 5s, shutting down")
            return

        goal_msg: DoObjectiveSequence.Goal = DoObjectiveSequence.Goal(
            objective_xml_string=objective_xml_string
        )

        # Send the goal and return the future for the goal handle
        send_goal_future: Future = action_client.send_goal_async(goal_msg)
        rclpy.spin_until_future_complete(node, send_goal_future)

        # Check if the goal has been accepted
        goal_handle = send_goal_future.result()
        if not goal_handle or not goal_handle.accepted:
            node.get_logger().error("Objective rejected by server")
            return

        # Wait for the result
        result_future: Future = goal_handle.get_result_async()
        rclpy.spin_until_future_complete(node, result_future)

        result_response: Optional[Any] = result_future.result()
        if result_response is None:
            node.get_logger().error("Failed to get result from action server")
            return

        result: DoObjectiveSequence.Result = result_response.result

        if result.error_code.val == 1:  # MoveIt SUCCESS
            node.get_logger().info("Objective completed successfully!")
        else:
            node.get_logger().error("Objective failed!")
            node.get_logger().error(f"Error code: {result.error_code.val}")
            node.get_logger().error(f"Error message: {result.error_message}")

    except KeyboardInterrupt:
        node.get_logger().info("Interrupted by user")
        if goal_handle:
            goal_handle.cancel_goal_async()
    except Exception as e:
        node.get_logger().error(f"Unexpected error: {e}")
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

tip

A list of all Error Codes can be found at the MoveIt Messages Documentation

Existing Objectives

If the objective already exists, instead of sending the objective in XML format, we can send a goal using the existing objective name.

goal_msg: DoObjectiveSequence.Goal = DoObjectiveSequence.Goal(
    objective_name="Close Gripper"
)

Running an Objective from the Shell

As shown previously, you can send a full Objective via a string directly to MoveIt Pro. When you have an instance of MoveIt Pro running, attach a shell to the Docker container by running moveit_pro shell. From the shell, run the following command:

ros2 action send_goal /do_objective moveit_studio_sdk_msgs/action/DoObjectiveSequence '
objective_xml_string: |
  <root BTCPP_format="4" main_tree_to_execute="test simple">
    <BehaviorTree ID="test simple">
      <Control ID="Sequence" name="TopLevelSequence">
        <Action ID="AlwaysSuccess"/>
      </Control>
    </BehaviorTree>
  </root>
'

You should see the Objective successfully run in the UI.

It is also possible to run an Objective or subtree that already exists while also remapping ports! Below, is an Objective that wraps the Move To Waypoint subtree. The waypoint_name port is mapped to Look at Table (This Objective assumes you are running the lab_sim config). Try running the Objective, you should see the arm move to the Look at Table waypoint.

ros2 action send_goal /do_objective moveit_studio_sdk_msgs/action/DoObjectiveSequence '
objective_xml_string: |
  <root BTCPP_format="4" main_tree_to_execute="Move To Waypoint Remapped">
    <BehaviorTree ID="Move To Waypoint Remapped">
      <Control ID="Sequence" name="TopLevelSequence">
        <SubTree
          ID="Move to Waypoint"
          _collapsed="true"
          waypoint_name="Look at Table"
          joint_group_name="manipulator"
          controller_names="/joint_trajectory_controller /robotiq_gripper_controller"
          use_all_planners="false"
        />
      </Control>
    </BehaviorTree>
  </root>
'

Now if you take the same code above but set waypoint_name to Park Far Right, you should see the arm move all the way to the right. This functionality gives you the ability to invoke MoveIt Pro Objectives and Behaviors with more flexibility.

For existing objectives, you can use the objective name as well.

ros2 action send_goal /do_objective moveit_studio_sdk_msgs/action/DoObjectiveSequence "{objective_name: 'Close Gripper'}"