ROS2规划系统plansys2简单的例子

ROS2规划系统ros2_planning_system

---

安装：

sudo apt install ros-foxy-plansys2-bringup ros-foxy-plansys2-bt-actions ros-foxy-plansys2-core ros-foxy-plansys2-domain-expert ros-foxy-plansys2-executor ros-foxy-plansys2-lifecycle-manager ros-foxy-plansys2-msgs ros-foxy-plansys2-pddl-parser ros-foxy-plansys2-planner ros-foxy-plansys2-popf-plan-solver ros-foxy-plansys2-problem-expert ros-foxy-plansys2-terminal

如果使用galactic，替换foxy即可。

编译准备：

rosdep install --from-paths src -r -y

#All required rosdeps installed successfully

编译：

colcon build --symlink-install
source install/setup.bash 

---

运行案例（参考plansys2_simple_example_launch.py）：

# Copyright 2019 Intelligent Robotics Lab
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    # Get the launch directory
    example_dir = get_package_share_directory('plansys2_simple_example')
    namespace = LaunchConfiguration('namespace')

    declare_namespace_cmd = DeclareLaunchArgument(
        'namespace',
        default_value='',
        description='Namespace')

    stdout_linebuf_envvar = SetEnvironmentVariable(
        'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')

    plansys2_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(
            get_package_share_directory('plansys2_bringup'),
            'launch',
            'plansys2_bringup_launch_monolithic.py')),
        launch_arguments={
          'model_file': example_dir + '/pddl/simple_example.pddl',
          'namespace': namespace
          }.items())

    # Specify the actions
    move_cmd = Node(
        package='plansys2_simple_example',
        executable='move_action_node',
        name='move_action_node',
        namespace=namespace,
        output='screen',
        parameters=[])

    charge_cmd = Node(
        package='plansys2_simple_example',
        executable='charge_action_node',
        name='charge_action_node',
        namespace=namespace,
        output='screen',
        parameters=[])

    ask_charge_cmd = Node(
        package='plansys2_simple_example',
        executable='ask_charge_action_node',
        name='ask_charge_action_node',
        namespace=namespace,
        output='screen',
        parameters=[])   # Create the launch description and populate
    ld = LaunchDescription()

    # Set environment variables
    ld.add_action(stdout_linebuf_envvar)
    ld.add_action(declare_namespace_cmd)

    # Declare the launch options
    ld.add_action(plansys2_cmd)

    ld.add_action(move_cmd)
    ld.add_action(charge_cmd)
    ld.add_action(ask_charge_cmd)

    return ld

---

命令行运行格式：

set instance leia robot
set instance entrance room
set instance kitchen room
set instance bedroom room
set instance dinning room
set instance bathroom room
set instance chargingroom room

set predicate (connected entrance dinning)
set predicate (connected dinning entrance)

set predicate (connected dinning kitchen)
set predicate (connected kitchen dinning)

set predicate (connected dinning bedroom)
set predicate (connected bedroom dinning)

set predicate (connected bathroom bedroom)
set predicate (connected bedroom bathroom)

set predicate (connected chargingroom kitchen)
set predicate (connected kitchen chargingroom)

set predicate (charging_point_at chargingroom)
set predicate (battery_low leia)
set predicate (robot_at leia entrance)


set goal (and(robot_at leia bathroom))

run

get plan

run (move leia entrance dinning)
run (askcharge leia entrance)

run (move leia bedroom kitchen)

remove predicate battery_full leia
set predicate battery_low leia


run --ros-args -p model_file:=/home/ros/RobCode/ros2_planning_system_examples/src/plansys2_simple_example/pddl/simple_example.pddl 

---

(define (domain simple)
(:requirements :strips :typing :adl :fluents :durative-actions)

;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(:types
robot
room
);; end Types ;;;;;;;;;;;;;;;;;;;;;;;;;

;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;;
(:predicates

(robot_at ?r - robot ?ro - room)
(connected ?ro1 ?ro2 - room)
(battery_full ?r - robot)
(battery_low ?r - robot)
(charging_point_at ?ro - room)

);; end Predicates ;;;;;;;;;;;;;;;;;;;;
;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;;
(:functions

);; end Functions ;;;;;;;;;;;;;;;;;;;;
;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(:durative-action move
    :parameters (?r - robot ?r1 ?r2 - room)
    :duration ( = ?duration 5)
    :condition (and
        (at start(connected ?r1 ?r2))
        (at start(robot_at ?r ?r1))
        (over all(battery_full ?r))
        )
    :effect (and
        (at start(not(robot_at ?r ?r1)))
        (at end(robot_at ?r ?r2))
    )
)

(:durative-action askcharge
    :parameters (?r - robot ?r1 ?r2 - room)
    :duration ( = ?duration 5)
    :condition (and
        (at start(robot_at ?r ?r1))
        (at start(charging_point_at ?r2))
       )
    :effect (and
        (at start(not(robot_at ?r ?r1)))
        (at end(robot_at ?r ?r2))
    )
)

(:durative-action charge
    :parameters (?r - robot ?ro - room)
    :duration ( = ?duration 5)
    :condition (and
        (at start(robot_at ?r ?ro))
        (at start(charging_point_at ?ro))
    )
    :effect (and
         (at end(not(battery_low ?r)))
         (at end(battery_full ?r))
    )
)

);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;

 

---

执行过程模拟：

// Copyright 2019 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <memory>
#include <algorithm>

#include "plansys2_executor/ActionExecutorClient.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

using namespace std::chrono_literals;

class AskCharge : public plansys2::ActionExecutorClient
{
public:
  AskCharge()
  : plansys2::ActionExecutorClient("askcharge", 1s)
  {
    progress_ = 0.0;
  }

private:
  void do_work()
  {
    if (progress_ < 1.0) {
      progress_ += 0.05;
      send_feedback(progress_, "AskCharge running");
    } else {
      finish(true, 1.0, "AskCharge completed");

      progress_ = 0.0;
      std::cout << std::endl;
    }

    std::cout << "\r\e[K" << std::flush;
    std::cout << "Requesting for charging ... [" << std::min(100.0, progress_ * 100.0) << "%]  " <<
      std::flush;
  }

  float progress_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<AskCharge>();

  node->set_parameter(rclcpp::Parameter("action_name", "askcharge"));
  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);

  rclcpp::spin(node->get_node_base_interface());

  rclcpp::shutdown();

  return 0;
}

这里面使用：

  {
    if (progress_ < 1.0) {
      progress_ += 0.05;
      send_feedback(progress_, "AskCharge running");
    } else {
      finish(true, 1.0, "AskCharge completed");

      progress_ = 0.0;
      std::cout << std::endl;
    }

    std::cout << "\r\e[K" << std::flush;
    std::cout << "Requesting for charging ... [" << std::min(100.0, progress_ * 

 一个加法累积，模拟进度，到100%，显示完成。

有三个，还有一个是0.02，如下：

// Copyright 2019 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <memory>
#include <algorithm>

#include "plansys2_executor/ActionExecutorClient.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

using namespace std::chrono_literals;

class MoveAction : public plansys2::ActionExecutorClient
{
public:
  MoveAction()
  : plansys2::ActionExecutorClient("move", 250ms)
  {
    progress_ = 0.0;
  }

private:
  void do_work()
  {
    if (progress_ < 1.0) {
      progress_ += 0.02;
      send_feedback(progress_, "Move running");
    } else {
      finish(true, 1.0, "Move completed");

      progress_ = 0.0;
      std::cout << std::endl;
    }

    std::cout << "\r\e[K" << std::flush;
    std::cout << "Moving ... [" << std::min(100.0, progress_ * 100.0) << "%]  " <<
      std::flush;
  }

  float progress_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<MoveAction>();

  node->set_parameter(rclcpp::Parameter("action_name", "move"));
  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);

  rclcpp::spin(node->get_node_base_interface());

  rclcpp::shutdown();

  return 0;
}

---

ros2 launch plansys2_simple_example plansys2_simple_example_launch.py
[INFO] [launch]: All log files can be found below /home/ros/.ros/log/2022-07-06-21-11-30-838227-ros-12774
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [plansys2_node-1]: process started with pid [12776]
[INFO] [move_action_node-2]: process started with pid [12778]
[INFO] [charge_action_node-3]: process started with pid [12780]
[INFO] [ask_charge_action_node-4]: process started with pid [12782]
[charge_action_node-3] RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED is now ignored.  Please set RCUTILS_LOGGING_USE_STDOUT and RCUTILS_LOGGING_BUFFERED_STREAM to control the stream and the buffering of log messages.
[plansys2_node-1] RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED is now ignored.  Please set RCUTILS_LOGGING_USE_STDOUT and RCUTILS_LOGGING_BUFFERED_STREAM to control the stream and the buffering of log messages.
[move_action_node-2] RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED is now ignored.  Please set RCUTILS_LOGGING_USE_STDOUT and RCUTILS_LOGGING_BUFFERED_STREAM to control the stream and the buffering of log messages.
[ask_charge_action_node-4] RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED is now ignored.  Please set RCUTILS_LOGGING_USE_STDOUT and RCUTILS_LOGGING_BUFFERED_STREAM to control the stream and the buffering of log messages.
[plansys2_node-1] [INFO] [1657113091.117842286] [domain_expert_lc_mngr]: Creating client for service [domain_expert/get_state]
[plansys2_node-1] [INFO] [1657113091.117985314] [domain_expert_lc_mngr]: Creating client for service [domain_expert/change_state]
[plansys2_node-1] [INFO] [1657113091.118944458] [executor_lc_mngr]: Creating client for service [executor/get_state]
[plansys2_node-1] [INFO] [1657113091.118978916] [executor_lc_mngr]: Creating client for service [executor/change_state]
[plansys2_node-1] [INFO] [1657113091.120056912] [planner_lc_mngr]: Creating client for service [planner/get_state]
[plansys2_node-1] [INFO] [1657113091.120104678] [planner_lc_mngr]: Creating client for service [planner/change_state]
[plansys2_node-1] [INFO] [1657113091.121063103] [problem_expert_lc_mngr]: Creating client for service [problem_expert/get_state]
[plansys2_node-1] [INFO] [1657113091.121096320] [problem_expert_lc_mngr]: Creating client for service [problem_expert/change_state]
[plansys2_node-1] [INFO] [1657113091.123246979] [domain_expert]: [domain_expert] Configuring...
[plansys2_node-1] [INFO] [1657113092.591281768] [domain_expert]: [domain_expert] Configured
[plansys2_node-1] [INFO] [1657113092.591821518] [domain_expert_lc_mngr]: Transition 1 successfully triggered.
[plansys2_node-1] [INFO] [1657113092.592735774] [domain_expert_lc_mngr]: Node domain_expert_lc_mngr has current state inactive.
[plansys2_node-1] [INFO] [1657113092.593341410] [problem_expert]: [problem_expert] Configuring...
[plansys2_node-1] [INFO] [1657113092.593592323] [problem_expert]: [problem_expert] Configured
[plansys2_node-1] [INFO] [1657113092.593980188] [problem_expert_lc_mngr]: Transition 1 successfully triggered.
[plansys2_node-1] [INFO] [1657113092.594893820] [problem_expert_lc_mngr]: Node problem_expert_lc_mngr has current state inactive.
[plansys2_node-1] [INFO] [1657113092.595116986] [planner]: [planner] Configuring...
[plansys2_node-1] [INFO] [1657113092.595572154] [planner]: Created solver : POPF of type plansys2/POPFPlanSolver
[plansys2_node-1] [INFO] [1657113092.595597202] [planner]: [planner] Configured
[plansys2_node-1] [INFO] [1657113092.596173571] [planner_lc_mngr]: Transition 1 successfully triggered.
[plansys2_node-1] [INFO] [1657113092.597047621] [planner_lc_mngr]: Node planner_lc_mngr has current state inactive.
[plansys2_node-1] [INFO] [1657113092.597266099] [executor]: [executor] Configuring...
[plansys2_node-1] [INFO] [1657113092.635329147] [executor]: [executor] Configured
[plansys2_node-1] [INFO] [1657113092.636003817] [executor_lc_mngr]: Transition 1 successfully triggered.
[plansys2_node-1] [INFO] [1657113092.637613423] [executor_lc_mngr]: Node executor_lc_mngr has current state inactive.
[plansys2_node-1] [INFO] [1657113092.637831052] [domain_expert]: [domain_expert] Activating...
[plansys2_node-1] [INFO] [1657113092.637846397] [domain_expert]: [domain_expert] Activated
[plansys2_node-1] [INFO] [1657113092.638013755] [domain_expert_lc_mngr]: Transition 3 successfully triggered.
[plansys2_node-1] [INFO] [1657113092.638161275] [problem_expert]: [problem_expert] Activating...
[plansys2_node-1] [INFO] [1657113092.638173395] [problem_expert]: [problem_expert] Activated
[plansys2_node-1] [INFO] [1657113092.638334715] [problem_expert_lc_mngr]: Transition 3 successfully triggered.
[plansys2_node-1] [INFO] [1657113092.638480864] [planner]: [planner] Activating...
[plansys2_node-1] [INFO] [1657113092.638492468] [planner]: [planner] Activated
[plansys2_node-1] [INFO] [1657113092.638652933] [planner_lc_mngr]: Transition 3 successfully triggered.
[plansys2_node-1] [INFO] [1657113092.638833961] [executor]: [executor] Activating...
[plansys2_node-1] [INFO] [1657113092.638846389] [executor]: [executor] Activated
[plansys2_node-1] [INFO] [1657113092.639011505] [executor_lc_mngr]: Transition 3 successfully triggered.
[plansys2_node-1] [INFO] [1657113092.639378348] [domain_expert_lc_mngr]: Node domain_expert_lc_mngr has current state active.
[plansys2_node-1] [INFO] [1657113092.639805597] [problem_expert_lc_mngr]: Node problem_expert_lc_mngr has current state active.
[plansys2_node-1] [INFO] [1657113092.639997659] [planner_lc_mngr]: Node planner_lc_mngr has current state active.
[plansys2_node-1] [INFO] [1657113092.640170439] [executor_lc_mngr]: Node executor_lc_mngr has current state active.
[plansys2_node-1] Critical Errors Encountered in Domain/Problem File
[plansys2_node-1] --------------------------------------------------
[plansys2_node-1] 
[plansys2_node-1] Due to critical errors in the supplied domain/problem file, the planner
[plansys2_node-1] has to terminate.  The errors encountered are as follows:
[plansys2_node-1] [INFO] [1657113223.236959183] [executor]: Action askcharge timeout percentage -1.000000
[plansys2_node-1] [INFO] [1657113223.238439887] [executor]: Action charge timeout percentage -1.000000
[plansys2_node-1] [INFO] [1657113223.240075646] [executor]: Action move timeout percentage -1.000000
[plansys2_node-1] [INFO] [1657113223.241757081] [executor]: Action move timeout percentage -1.000000
[plansys2_node-1] [INFO] [1657113223.243454177] [executor]: Action move timeout percentage -1.000000
[plansys2_node-1] [INFO] [1657113223.245104087] [executor]: Action move timeout percentage -1.000000
[plansys2_node-1] [WARN] [1657113223.248743929] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[plansys2_node-1] [WARN] [1657113223.260558208] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
Requesting for charging ... [100%]  
Charging ... [100%]  
Moving ... [100%]  
Moving ... [100%]  
Moving ... [100%]  
Moving ... [100%]  
[plansys2_node-1] [INFO] [1657113304.791210722] [executor]: Plan Succeeded
[plansys2_node-1] [PublisherZMQ] Server quitting.
[plansys2_node-1] [PublisherZMQ] just died. Exception Context was terminated
[plansys2_node-1] [PublisherZMQ] Publisher quitting.
[plansys2_node-1] [PublisherZMQ] just died. Exception Context was terminated
 

---