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

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

; 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 cx_bringup package. Run it via:

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:

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:

ros2 run cx_bringup test_service.py

Configuration

File cx_bringup/params/plugin_examples/fibonacci_action.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 cx_bringup/clips/plugin_examples/fibonacci-action.clp.

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