.. _configuring_gen_action:

Generated ROS Action Plugin
###########################

Source code generated by :docsite:`cx_ros_comm_gen`.

Given the *package* (e.g., ``example_interfaces``) and *action_name* (e.g., ``Fibonacci``), this document uses the following abbreviations:

 - **PkgCamel**: CamelCase of *package* (e.g., ``ExampleInterfaces``)
 - **SrvCamel**: CamelCase of *action_name* (e.g., ``Fibonacci``)
 - **pkg-kebab**: kebab-case of *package* (e.g., ``example-interfaces``)
 - **action-kebab**: kebab-case of *action_name* (e.g., ``fibonacci``)

.. admonition:: Plugin Class

  cx::CX\ **PkgCamelACtionCamel**\ Plugin

  (e.g., cx::CXExampleInterfacesFibonacciPlugin)

Configuration
*************

This plugin has no specific configuration.

Features
********

Facts
~~~~~

.. code-block:: lisp

  ; Asserted by the respective creat-client function.
  ; Retracted by the respective destroy-client function.
  (<package-kebab>-<action-kebab>-client (server ?server-name-string))

  ; Asserted by the respective create-server function.
  ; Retracted by the respective destroy-server function.
  (<package-kebab>-<action-kebab>-server (name ?server-name-string))

  ; Asserted by the goal response callback of a client.
  ; Process the response and then destroy feedback.
  ; The client-goal-handle should persist until the processing of the request is fully done.
  (<package-kebab>-<action-kebab>-goal-response (server ?server-name-string) (client-goal-handle-ptr ?cgh-ptr))

  ; Asserted by the goal feedback callback of a client.
  ; Process the response and then destroy feedback (it is read-only, do not modify it).
  ; The client-goal-handle should persist (and not be destroyed) until the processing of the goal is fully done and the action is terminated.
  ; In particular, the client goal handle pointer here will match the one from the goal-response fact.
  (<package-kebab>-<action-kebab>-goal-feedback (server ?server-name-string) (client-goal-handle-ptr ?cgh-ptr) (feedback-ptr ?f-ptr))

  ; Asserted by the goal result callback of a client.
  ; Process the result and then destroy the result pointer before retracting this fact.
  ; "code" is either UNKNOWN SUCCEEDED CANCELED or ABORTED.
  ; "goal-id" is the string representation of a uuid according to rclcpp_action::to_string().
  (<package-kebab>-<action-kebab>-wrapped-result (server ?server-name-string) (goal-id ?uuid-str) (code ?code-symbol) (result-ptr ?f-ptr))

  ; Asserted by the goal result callback of a server.
  ; Process the server goal handle to retrieve important information such as the goal sent by the client.
  ; The server-goal-handle should persist (and not be destroyed) until the processing of the action is fully done and the action is terminated.
  (<package-kebab>-<action-kebab>-accepted-goal (server ?server-name-string) (server-goal-handle-ptr ?f-ptr))

Functions
~~~~~~~~~

.. code-block:: lisp

  ; Create and destroy clients and servers.
  ; After creating a server, user-defined function is called whenever a goal request arrives.
  (<package-kebab>-<action-kebab>-create-server ?server-name)
  (<package-kebab>-<action-kebab>-destroy-server ?server-name)
  (<package-kebab>-<action-kebab>-create-client ?server-name)
  (<package-kebab>-<action-kebab>-destroy-client ?server-name)

  ; Send a goal to an action server
  ; Requires the client to be created first using create-client.
  ; Callbacks will assert goal-response feedback and wrapped-result facts.
  ; Do not destroy the ?msg-ptr immediately, keep it until the goal is fully processed.
  (<package-kebab>-<action-kebab>-send-goal ?msg-ptr ?server-name)

  ; Creating, destroying and processing of goals
  (bind ?msg-ptr (<package-kebab>-<action-kebab>-goal-create))
  (<package-kebab>-<action-kebab>-goal-destroy ?msg-ptr)
  (<package-kebab>-<action-kebab>-goal-set-field ?msg-ptr ?field-name ?field-value)
  (<package-kebab>-<action-kebab>-goal-get-field ?msg-ptr ?field-name)

  ; Creating, destroying and processing of feedback
  (bind ?msg-ptr (<package-kebab>-<action-kebab>-feedback-create))
  (<package-kebab>-<action-kebab>-feedback-destroy ?msg-ptr)
  (<package-kebab>-<action-kebab>-feedback-set-field ?msg-ptr ?field-name ?field-value)
  (<package-kebab>-<action-kebab>-feedback-get-field ?msg-ptr ?field-name)

  ; Creating, destroying and processing of results
  (bind ?msg-ptr (<package-kebab>-<action-kebab>-result-create))
  (<package-kebab>-<action-kebab>-result-destroy ?msg-ptr)
  (<package-kebab>-<action-kebab>-result-set-field ?msg-ptr ?field-name ?field-value)
  (<package-kebab>-<action-kebab>-result-get-field ?msg-ptr ?field-name)

  ; Destroy server goal handle pointer.
  (<package-kebab>-<action-kebab>-server-goal-handle-destroy ?handle-ptr)
  ; server goal handle members (see rclcpp_action documentation)
  (<package-kebab>-<action-kebab>-server-goal-handle-abort ?handle-ptr ?result-ptr)
  (<package-kebab>-<action-kebab>-server-goal-handle-succeed ?handle-ptr ?result-ptr)
  (<package-kebab>-<action-kebab>-server-goal-handle-canceled ?handle-ptr ?result-ptr)
  (bind ?goal-ptr (<package-kebab>-<action-kebab>-server-goal-handle-get-goal ?handle-ptr))
  ; the goal id is returned as string representation via rclcpp_action::to_string()
  (bind ?uuid-str (<package-kebab>-<action-kebab>-server-goal-handle-get-goal-id ?handle-ptr))
  (bind ?bool-sym (<package-kebab>-<action-kebab>-server-goal-handle-is-canceling ?handle-ptr))
  (bind ?bool-sym (<package-kebab>-<action-kebab>-server-goal-handle-is-active ?handle-ptr))
  (bind ?bool-sym (<package-kebab>-<action-kebab>-server-goal-handle-is-executing ?handle-ptr))
  (<package-kebab>-<action-kebab>-server-goal-handle-execute ?handle-ptr)
  (<package-kebab>-<action-kebab>-server-goal-handle-publish-feedback ?handle-ptr ?feedback-ptr)

  ; client goal handle members (see rclcpp_action documentation)
  (bind ?bool-sym (<package-kebab>-<action-kebab>-client-goal-handle-is-feedback-aware ?handle-ptr))
  (bind ?bool-sym (<package-kebab>-<action-kebab>-client-goal-handle-is-result-aware ?handle-ptr))
  ; return codes:
  ; 0 = STATUS_UNKNOWN
  ; 1 = STATUS_ACCEPTED
  ; 2 = STATUS_EXECUTING
  ; 3 = STATUS_CANCELING
  ; 4 = STATUS_SUCCEEDED
  ; 5 = STATUS_CANCELED
  ; 6 = STATUS_ABORTED
  (bind ?status-int (<package-kebab>-<action-kebab>-client-goal-handle-get-status ?handle-ptr))
  ; the goal id is returned as string representation via rclcpp_action::to_string()
  (bind ?uuid-str (<package-kebab>-<action-kebab>-client-goal-handle-get-goal-id ?handle-ptr))
  (bind ?time-seconds-float (<package-kebab>-<action-kebab>-client-goal-handle-get-goal-stamp ?handle-ptr))

Functions Defined by User
~~~~~~~~~~~~~~~~~~~~~~~~~

The following functions are called at the appropriate places if they are defined by the user.

.. code-block:: lisp

  ; Gets called for each server receiving a goal, needs to return one of these integers:
  ; (return 1) ; REJECT
  ; (return 2) ; ACCEPT_AND_EXECUTE
  ; (return 3) ; ACCEPT_AND_DEFER
  ; If the function does not exist, every goal is automatically accepted (ACCEPT_AND_EXECUTE)
  (bind ?response-int (<package-kebab>-<action-kebab>-handle-goal-callback ?server-name-str ?goal-ptr ?uuid-str))

; Gets called for each server receiving a cancellation request, needs to return one of these integers:
; (return 0) ; REJECT
; (return 1) ; ACCEPT
; If the function does not exist, every request is automatically accepted (ACCEPT)
(bind ?response-int (<package-kebab>-<action-kebab>-cancel-goal-callback ?server-name-str ?server-goal-handle-ptr))

Usage Example
*************

A minimal working example is provided by the :docsite:`cx_bringup` package. Run it via:

.. code-block:: bash

    ros2 launch cx_bringup cx_launch.py manager_config:=plugin_examples/fibonacci_action.yaml

It creates a ``std_srvs/srv/SetBool`` service ``/ros_cx_srv``. The services answers by giving the requested value in success.

A simple service call can be made using the ROS CLI tool:

.. code-block:: bash

  ros2 service call /ros_cx_srv std_srvs/srv/SetBool "{data: false}"

Additionally, if ``true`` was requested, the example makes a new request with data ``true`` to a service called ``/ros_cx_srv_client``.
To start a simple server accepting the request, simply run the following command:

.. code-block:: bash

    ros2 run cx_bringup test_service.py


Configuration
~~~~~~~~~~~~~

File :source-master:`cx_bringup/params/plugin_examples/fibonacci_action.yaml`.

.. code-block:: yaml

  clips_manager:
    ros__parameters:
      environments: ["cx_fibonacci_action"]
      cx_fibonacci_action:
        plugins: ["executive", "fibonacci", "files"]
        log_clips_to_file: true
        watch: ["facts", "rules"]

      executive:
        plugin: "cx::ExecutivePlugin"
        publish_on_refresh: false
        assert_time: true
        refresh_rate: 10
      fibonacci:
        plugin: "cx::CXExampleInterfacesFibonacciPlugin"
      files:
        plugin: "cx::FileLoadPlugin"
        pkg_share_dirs: ["cx_bringup"]
        load: [
          "clips/plugin_examples/fibonacci-action.clp"]



Code
~~~~

File :source-master:`cx_bringup/clips/plugin_examples/fibonacci-action.clp`.

.. code-block:: lisp

  (deffunction example-interfaces-fibonacci-handle-goal-callback (?server ?goal ?uuid)
    (printout blue ?server " callback (goal " ?goal " ; id " ?uuid  " )" crlf)
    ; (return 1) ; REJECT
    (return 2) ; ACCEPT_AND_EXECUTE
    ; (return 3) ; ACCEPT_AND_DEFER
  )

  (deffunction example-interfaces-fibonacci-cancel-goal-callback (?server ?goal ?goal-handle)
    ; (return 0) ; REJECT
    (return 1) ; ACCEPT
  )

  (defrule fibonacci-action-client-server-init
  " Create a simple client and service using the generated bindings. "
    (not (example-interfaces-fibonacci-client (server "ros_cx_fibonacci")))
    (not (example-interfaces-fibonacci-server (name "ros_cx_fibonacci")))
    (not (executive-finalize))
    ;(not (client-server-already-created))
  =>
    ;(assert (client-server-already-created))
    (example-interfaces-fibonacci-create-client "ros_cx_fibonacci")
    (printout info "Created client for /ros_cx_fibonacci" crlf)
    (example-interfaces-fibonacci-create-server "ros_cx_fibonacci")
    (printout info "Created server for /ros_cx_fibonacci" crlf)
  )

  (deftemplate fibonacci
    (slot uuid (type STRING))
    (slot order (type INTEGER))
    (slot progress (type INTEGER))
    (multislot sequence (type INTEGER))
    (slot result (type INTEGER))
    (slot last-computed (type FLOAT))
  )

  (defrule fibonacci-goal-accepted-start-compute
    (example-interfaces-fibonacci-accepted-goal (server ?server) (server-goal-handle-ptr ?ptr))
    (not (fibonacci (uuid ?uuid&:(eq ?uuid (example-interfaces-fibonacci-server-goal-handle-get-goal-id ?ptr)))))
    =>
    (if (not (example-interfaces-fibonacci-server-goal-handle-is-canceling ?ptr)) then
      (bind ?goal (example-interfaces-fibonacci-server-goal-handle-get-goal ?ptr))
      (bind ?order (example-interfaces-fibonacci-goal-get-field ?goal "order"))
      (bind ?uuid (example-interfaces-fibonacci-server-goal-handle-get-goal-id ?ptr))
      (assert (fibonacci (uuid ?uuid) (order ?order) (progress 2) (result 0) (sequence (create$ 0 1)) (last-computed (now))))
     else
      (printout error "Somehow the goal is canceling already" crlf)
    )
    ; do not destroy the server goal handle here, only do it once the goal is fully processed and finished
    ; (example-interfaces-fibonacci-server-goal-handle-destroy ?ptr)
  )

  (defrule fibonacci-compute-next
    (example-interfaces-fibonacci-accepted-goal (server ?server) (server-goal-handle-ptr ?ptr))
    ?f <- (fibonacci (order ?order) (progress ?remaining&:(>= ?order ?remaining))
    (last-computed ?computed) (result ?old-res) (sequence $?seq) (uuid ?uuid))
    (time ?now&:(> (- ?now ?computed) 1))
    (test (eq ?uuid (example-interfaces-fibonacci-server-goal-handle-get-goal-id ?ptr)))
    =>
    (bind ?step (+ ?remaining 1))
    (bind ?res (+ (nth$ ?remaining ?seq) (nth$ (- ?remaining 1) ?seq)))
    (printout magenta "Computing partial result fibonacci(" ?remaining ") = " ?res crlf)
    (bind ?seq (create$ ?seq ?res))
    (modify ?f (progress ?step) (result (+ ?old-res ?res)) (sequence ?seq))
    (bind ?feedback (example-interfaces-fibonacci-feedback-create))
    (example-interfaces-fibonacci-feedback-set-field ?feedback "sequence" ?seq)
    (example-interfaces-fibonacci-server-goal-handle-publish-feedback ?ptr ?feedback)
    (example-interfaces-fibonacci-feedback-destroy ?feedback)
    (modify ?f (last-computed ?now))
  )

  (defrule fibonacci-compute-done
    ?ag <- (example-interfaces-fibonacci-accepted-goal (server ?server) (server-goal-handle-ptr ?ptr))
    ?f <- (fibonacci (order ?order) (progress ?remaining&:(< ?order ?remaining)) (result ?old-res) (sequence $?seq) (uuid ?uuid&:(eq ?uuid (example-interfaces-fibonacci-server-goal-handle-get-goal-id ?ptr))))
    =>
    (printout green "Final fibonacci sequence (server): " ?seq crlf)
    (bind ?result (example-interfaces-fibonacci-result-create))
    (example-interfaces-fibonacci-result-set-field ?result "sequence" ?seq)
    (example-interfaces-fibonacci-server-goal-handle-succeed ?ptr ?result)
    (example-interfaces-fibonacci-result-destroy ?result)
    (example-interfaces-fibonacci-server-goal-handle-destroy ?ptr)
    (retract ?f)
    (retract ?ag)
  )

  (defrule fibonacci-client-send-goal
    (example-interfaces-fibonacci-client (server ?server))
    (not (send-request))
    =>
    (assert (send-request))
    (bind ?goal (example-interfaces-fibonacci-goal-create))
    (assert (fibnoacci-goal ?goal))
    (example-interfaces-fibonacci-goal-set-field ?goal "order" 5)
    (example-interfaces-fibonacci-send-goal ?goal ?server)

    ; do not destroy the goal here, only do it once the goal is fully processed and finished
    ; (example-interfaces-fibonacci-goal-destroy ?goal)
  )

  (defrule fibonacci-client-get-feedback
    (declare (salience 100))
    ?f <- (example-interfaces-fibonacci-goal-feedback (server ?server) (client-goal-handle-ptr ?ghp) (feedback-ptr ?fp))
    =>
    (bind ?g-id (example-interfaces-fibonacci-client-goal-handle-get-goal-id ?ghp))
    (bind ?g-stamp (example-interfaces-fibonacci-client-goal-handle-get-goal-stamp ?ghp))
    (bind ?g-status (example-interfaces-fibonacci-client-goal-handle-get-status ?ghp))
    (bind ?g-is-f-aware (example-interfaces-fibonacci-client-goal-handle-is-feedback-aware ?ghp))
    (bind ?g-is-r-aware (example-interfaces-fibonacci-client-goal-handle-is-result-aware ?ghp))
    ; the stamp seems to be broken (looks like a rclcpp_action issue)
    (printout cyan "[" (- (now) ?g-stamp) "] " ?g-status " " ?g-id " f " ?g-is-f-aware " r " ?g-is-r-aware crlf)
    (bind ?part-seq (example-interfaces-fibonacci-feedback-get-field ?fp "sequence"))
    (printout blue "partial sequence: " ?part-seq   crlf)
    (example-interfaces-fibonacci-feedback-destroy ?fp)
    (retract ?f)
  )

  (defrule fibonacci-client-cleanup-after-wrapped-result
    (declare (salience 10))
    ?f <- (example-interfaces-fibonacci-goal-response (server ?server) (client-goal-handle-ptr ?ghp))
    ?g <- (example-interfaces-fibonacci-wrapped-result (server ?server) (goal-id ?uuid) (code SUCCEEDED) (result-ptr ?rp))
    ?request-goal <- (fibnoacci-goal ?goal)
    (time ?now)
    =>
    (bind ?g-status (example-interfaces-fibonacci-client-goal-handle-get-status ?ghp))
    (if (> ?g-status 3) then ; status is final in one way or another
      (bind ?seq (example-interfaces-fibonacci-result-get-field ?rp "sequence"))
      (printout green "Final fibonacci sequence (client): " ?seq crlf)
      (example-interfaces-fibonacci-result-destroy ?rp)
      (retract ?g)
      (bind ?g-id (example-interfaces-fibonacci-client-goal-handle-get-goal-id ?ghp))
      (bind ?g-stamp (example-interfaces-fibonacci-client-goal-handle-get-goal-stamp ?ghp))
      (bind ?g-is-f-aware (example-interfaces-fibonacci-client-goal-handle-is-feedback-aware ?ghp))
      (bind ?g-is-r-aware (example-interfaces-fibonacci-client-goal-handle-is-result-aware ?ghp))
      (printout cyan "Final goal response [" (- (now) ?g-stamp) "] " ?uuid " " ?g-status " " ?g-id " f " ?g-is-f-aware " r " ?g-is-r-aware crlf)
      (example-interfaces-fibonacci-client-goal-handle-destroy ?ghp)
      (retract ?f)
      (example-interfaces-fibonacci-goal-destroy ?goal)
      (retract ?request-goal)
    )
  )

  (defrule fibonacci-client-server-cleanup
    (executive-finalize)
    (example-interfaces-fibonacci-client (server ?client))
    (example-interfaces-fibonacci-server (name ?server))
    =>
    (example-interfaces-fibonacci-destroy-client ?client)
    (example-interfaces-fibonacci-destroy-server ?server)
  )

  (defrule fibonacci-goal-response-cleanup
    (executive-finalize)
    ?f <- (example-interfaces-fibonacci-goal-response (client-goal-handle-ptr ?p))
    =>
    (example-interfaces-fibonacci-client-goal-handle-destroy ?p)
    (retract ?f)
  )

  (defrule fibonacci-accepted-goal-cleanup
    (executive-finalize)
    ?f <- (example-interfaces-fibonacci-accepted-goal (server-goal-handle-ptr ?p))
    =>
    (example-interfaces-fibonacci-server-goal-handle-destroy ?p)
    (retract ?f)
  )

  (defrule fibonacci-accepted-goal-cleanup
    (executive-finalize)
    ?f <- (fibnoacci-goal p)
    =>
    (example-interfaces-fibonacci--goal-destroy ?p)
    (retract ?f)
  )