Generated ROS Action Plugin

Source code generated by 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)

Plugin Class

cx::CXPkgCamelACtionCamelPlugin

(e.g., cx::CXExampleInterfacesFibonacciPlugin)

Configuration

This plugin has no specific configuration.

Features

Facts

; Asserted by the respective create-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))

; Asserted by the cancel callback of the client.
; The return code is one of ERROR_NONE, ERROR_REJECTED, ERROR_UNKNOWN_GOAL_ID, ERROR_GOAL_TERMINATED.
; The num-goals slot indicates the amount of goals canceling.
(<package-kebab>-<action-kebab>-cancel-goal-response (server ?server-name-string) (return-code ?code-symbol) (num-goals ?n-int))
; Asserted by the cancel callback of the client.
; Provides information about which goals are actually canceling.
(<package-kebab>-<action-kebab>-cancel-goal-entry (server ?server-name-string) (goal-id ?uuid-string) (stamp ?stamp-float))

Functions

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

; Cancel goals
(<package-kebab>-<action-kebab>-client-cancel-goal ?server-name ?goal-handle-ptr)
(<package-kebab>-<action-kebab>-client-cancel-all-goals ?server-name)
(<package-kebab>-<action-kebab>-client-cancel-goals-before ?server-name ?stamp-sec-float)

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

; Destroy client goal handle pointer.
(<package-kebab>-<action-kebab>-client-goal-handle-destroy ?handle-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))
(<package-kebab>-<action-kebab>-client-goal-handle-stop-callbacks ?server-name ?handle-ptr)

Functions Defined by User

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

; 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 1) ; REJECT ; (return 2) ; 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 cx_bringup package. Run it via:

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

The launch file starts two CLIPS environments:

• One environment provides an action server that computes Fibonacci sequences.

• The second environment acts as an action client. It first requests a Fibonacci sequence of order 5. After the goal completes, it sends a second request for a sequence of order 10. The second goal is canceled after the sixth element has been computed.

Configuration

File cx_bringup/params/plugin_examples/fibonacci_action.yaml.

/**:
  ros__parameters:
    environments: ["cx_fibonacci_server", "cx_fibonacci_client"]

    cx_fibonacci_client:
      plugins: ["executive", "fibonacci", "files_client"]
      log_clips_to_file: true
      watch: ["facts", "rules"]
      redirect_stdout_to_debug: true

    cx_fibonacci_server:
      plugins: ["executive", "fibonacci", "files_server"]
      log_clips_to_file: true
      watch: ["facts", "rules"]
      redirect_stdout_to_debug: true

    executive:
      plugin: "cx::ExecutivePlugin"
      publish_on_refresh: false
      assert_time: true
      refresh_rate: 10

    fibonacci:
      plugin: "cx::CXExampleInterfacesFibonacciPlugin"

    files_client:
      plugin: "cx::FileLoadPlugin"
      pkg_share_dirs: ["cx_bringup"]
      load: [
        "clips/plugin_examples/fibonacci-action-client.clp"]

    files_server:
      plugin: "cx::FileLoadPlugin"
      pkg_share_dirs: ["cx_bringup"]
      load: [
        "clips/plugin_examples/fibonacci-action-server.clp"]

Code

File cx_bringup/clips/plugin_examples/fibonacci-action-server.clp.

(deftemplate fibonacci
  (slot uuid (type STRING))
  (slot order (type INTEGER))
  (slot progress (type INTEGER))
  (multislot sequence (type INTEGER))
  (slot result (type INTEGER))
  (slot canceled (type SYMBOL) (allowed-values FALSE TRUE))
  (slot last-computed (type FLOAT))
)

(deffunction example-interfaces-fibonacci-handle-goal-callback (?server ?goal ?uuid)
" Overrides default callback, printing a message before accepting. "
  (printout blue "Accepting goal, uuid: " ?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)
" Overrides default callback, marking the associated fibonacci fact for cancelation. "
  (do-for-fact ((?f fibonacci)) (eq ?f:uuid (example-interfaces-fibonacci-server-goal-handle-get-goal-id ?goal-handle))
    (printout blue "Accepting cancelation of goal with uuid " ?f:uuid crlf)
    (modify ?f (canceled TRUE))
    (return 2) ; ACCEPT
  )
  (return 1) ; REJECT
)

(defrule fibonacci-action-server-init
" Create a simple server using the generated bindings. "
  (not (example-interfaces-fibonacci-server (name "ros_cx_fibonacci")))
  (not (executive-finalize))
=>
  (example-interfaces-fibonacci-create-server "ros_cx_fibonacci")
  (printout blue "Created server for /ros_cx_fibonacci" crlf)
)

(defrule fibonacci-goal-accepted-start-compute
" Create an initial fibonacci fact for an accepted goal. "
  (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
" Compute the next number in the sequence and send out feedback accordingly. "
  (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) (canceled FALSE))
  (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 blue "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
" Send the final goal result once the computation is finished. "
  ?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 blue "Final fibonacci sequence: " ?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-compute-canceled
" Stop computation and send the partial result in case the goal is canceled. "
  ?ag <- (example-interfaces-fibonacci-accepted-goal (server ?server) (server-goal-handle-ptr ?ptr))
  ?f <- (fibonacci (sequence $?seq) (canceled TRUE) (uuid ?uuid&:(eq ?uuid (example-interfaces-fibonacci-server-goal-handle-get-goal-id ?ptr))))
  =>
  (printout blue "Canceling fibonacci sequence: " ?seq crlf)
  (bind ?result (example-interfaces-fibonacci-result-create))
  (example-interfaces-fibonacci-result-set-field ?result "sequence" ?seq)
  (example-interfaces-fibonacci-server-goal-handle-canceled ?ptr ?result)
  (example-interfaces-fibonacci-result-destroy ?result)
  (example-interfaces-fibonacci-server-goal-handle-destroy ?ptr)
  (retract ?f)
  (retract ?ag)
)

(defrule fibonacci-server-cleanup
  (executive-finalize)
  (example-interfaces-fibonacci-server (name ?server))
  =>
  (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)
)

File cx_bringup/clips/plugin_examples/fibonacci-action-client.clp.

(defrule fibonacci-action-client-init
" Create a simple client using the generated bindings. "
  (not (example-interfaces-fibonacci-client (server "ros_cx_fibonacci")))
  (not (executive-finalize))
=>
  (example-interfaces-fibonacci-create-client "ros_cx_fibonacci")
  (printout green "Created client for /ros_cx_fibonacci" crlf)
)

(defrule fibonacci-client-send-goal
" Request computation of the fibonacci sequence of order 5. "
  (example-interfaces-fibonacci-client (server ?server))
  (not (send-request))
  =>
  (assert (send-request))
  (bind ?goal (example-interfaces-fibonacci-goal-create))
  (assert (fibonacci-goal ?goal))
  (example-interfaces-fibonacci-goal-set-field ?goal "order" 5)
  (example-interfaces-fibonacci-send-goal ?goal ?server)
  (printout green "Sending goal fibonacci(5)" crlf)

  ; 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
" Print any partial compuation result received so far and perpare cancelation once the received sequence is of length 7. "
  (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 debug "[" (- (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 green "partial sequence: " ?part-seq   crlf)
  (if (= (length$ ?part-seq) 7) then
     (assert (cancel-goal))
  )
  (example-interfaces-fibonacci-feedback-destroy ?fp)
  (retract ?f)
)

(defrule fibonacci-client-cleanup-after-wrapped-result
" Process result of finished compuatation, printing the final sequence. "
  (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 ?code) (result-ptr ?rp))
  (test (eq ?uuid (example-interfaces-fibonacci-client-goal-handle-get-goal-id ?ghp)))
  ?request-goal <- (fibonacci-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"))
    (if (= ?g-status 4) then
      (printout green "Final fibonacci sequence: " ?seq crlf)
    )
    (if (= ?g-status 5) then
      (printout green "Canceled fibonacci sequence: " ?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 debug "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-send-goal-that-cancels-later
" Send out a second goal for the fibonacci sequence of order 10 once the previous goal is completed. "
  (example-interfaces-fibonacci-client (server ?server))
  (send-request)
  (not (fibonacci-goal ?))
  (not (send-request-again))
  =>
  (printout green "Request fibonacci(10), will cancel before finish" crlf)
  (assert (send-request-again))
  (bind ?goal (example-interfaces-fibonacci-goal-create))
  (assert (fibonacci-goal ?goal))
  (example-interfaces-fibonacci-goal-set-field ?goal "order" 10)
  (example-interfaces-fibonacci-send-goal ?goal ?server)
)

(defrule fibonacci-request-cancel
" Send out a cancel request after the partial feedback provides a sequence of length 7. "
  (cancel-goal)
  (example-interfaces-fibonacci-client (server ?server))
  (example-interfaces-fibonacci-goal-response (server ?server) (client-goal-handle-ptr ?ghp))
=>
  (example-interfaces-fibonacci-client-cancel-goal ?server ?ghp)
  (printout green "Canceling current goal" crlf)
)

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

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