ROSPlan Battery Monitor

edit

Hi!

I'm currently trying to implement a battery monitor into my model using the rosplan sensing interface. In short, I'm wondering if there's a way to be monitoring the battery level while the robot is navigating to a waypoint. If the battery falls below a certain threshold, then I can divert the action and move the robot toward a charger. I've implemented this in SMACH but I'm curious if a similar mechanism can be made in ROSPlan. Here's a simplified domain for the robot

(define (domain test_domain)

(:requirements :strips :typing :fluents :disjunctive-preconditions :durative-actions :negative-preconditions)

(:types
    waypoint
    robot
)

(:predicates
    (robot_at ?v - robot ?wp - waypoint)
    (visited ?wp - waypoint)
    (battery_getting_low ?v - robot)
)

;; Move between any two waypoints
(:durative-action move_to_waypoint
    :parameters (?v - robot ?from ?to - waypoint)
    :duration ( = ?duration 120)
    :condition (and
        (at start (robot_at ?v ?from))
        (over all (not (battery_getting_low ?v))))
    :effect (and
        (at end (visited ?to))
        (at start (not (robot_at ?v ?from)))
        (at end (robot_at ?v ?to)))
)
)

Then, I have the following sensing.yaml files which is loaded into the rosplan_sensing_interface node

functions:
  - $(find test_rosplan)/scripts/sensing_functions.py
topics:
  robot_at:
    params:
      - roger
      - '*'
    topic: /ground_truth/state
    msg_type: nav_msgs/Odometry

  battery_getting_low:
    params:
      - roger
    topic: /battery/charge_level
    msg_type: std_msgs/Float32
    operation: msg.data < 5.0

However, it reports being unsolvable. Thanks in advance!