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）：
代码语言：javascript复制# 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命令行运行格式：
代码语言：javascript复制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 ;;;;;;;;;;;;;;;;;;;;;;;;执行过程模拟：
代码语言：javascript复制// 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 << "re[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;
}这里面使用：
代码语言：javascript复制 {
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 << "re[K" << std::flush;
std::cout << "Requesting for charging ... [" << std::min(100.0, progress_ * 一个加法累积，模拟进度，到100%，显示完成。
有三个，还有一个是0.02，如下：
代码语言：javascript复制// 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 << "re[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
