当前位置：网站首页>ROS2规划系统plansys2简单的例子
ROS2规划系统plansys2简单的例子
2022-07-07 03:36:00 【zhangrelay】
安装：
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
边栏推荐
- leetcode 509. Fibonacci number
- Algorithm --- bit count (kotlin)
- SQLMAP使用教程(四)实战技巧三之绕过防火墙
- MySQL binlog related commands
- AVL树的实现
- 栈题目：有效括号的嵌套深度
- Fullgc problem analysis and solution summary
- Modify the jupyter notebook file path
- Introduction to abnova's in vitro mRNA transcription workflow and capping method
- Initial experience of addresssanitizer Technology
猜你喜欢
Communication between non parent and child components
How can clothing stores make profits?
Abnova circulating tumor DNA whole blood isolation, genomic DNA extraction and analysis
MySQL service is missing from computer service
Kuboard can't send email and nail alarm problem is solved
.net 5 FluentFTP连接FTP失败问题：This operation is only allowed using a successfully authenticated context
$refs: get the element object or sub component instance in the component:
RuntimeError: CUDA error: CUBLAS_ STATUS_ ALLOC_ Failed when calling `cublascreate (handle) `problem solving
云备份项目
Sqlmap tutorial (IV) practical skills three: bypass the firewall
随机推荐
Several important steps to light up the display
弹性布局（二）
readonly 只读
How can clothing stores make profits?
Maze games based on JS
多线程与高并发（9）——AQS其他同步组件（Semaphore、ReentrantReadWriteLock、Exchanger）
Flexible layout (I)
Sqlmap tutorial (IV) practical skills three: bypass the firewall
Non empty verification of collection in SQL
Master-slave replication principle of MySQL
[Luogu p1971] rabbit and egg game (bipartite game)
栈题目：有效括号的嵌套深度
CompletableFuture使用详解
How to share the same storage among multiple kubernetes clusters
Big coffee gathering ｜ nextarch foundation cloud development meetup is coming
MySQL service is missing from computer service
Tumor immunotherapy research prosci Lag3 antibody solution
修改Jupyter Notebook文件路径
The currently released SKU (sales specification) information contains words that are suspected to have nothing to do with baby
Le Service MySQL manque dans le service informatique