当前位置：网站首页>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
边栏推荐
- Advantages of using net core / why
- Role of virtual machine
- 修改Jupyter Notebook文件路径
- JS small exercise ---- time sharing reminder and greeting, form password display hidden effect, text box focus event, closing advertisement
- FPGA course: application scenario of jesd204b (dry goods sharing)
- [Luogu p1971] rabbit and egg game (bipartite game)
- Composition API 前提
- PostgreSQL source code (59) analysis of transaction ID allocation and overflow judgment methods
- Abnova membrane protein lipoprotein technology and category display
- main函数在import语句中的特殊行为
猜你喜欢
Lvs+kept (DR mode) learning notes
Matlab tips (29) polynomial fitting plotfit
$parent(获取父组件) 和 $root(获取根组件)
$refs: get the element object or sub component instance in the component:
mips uclibc 交叉编译ffmpeg，支持 G711A 编解码
Complete process of MySQL SQL
Sword finger offer high quality code
Abnova immunohistochemical service solution
計算機服務中缺失MySQL服務
Maze games based on JS
随机推荐
transform-origin属性详解
Docker compose start redis cluster
[noi simulation] regional division (conclusion, structure)
软件验收测试
异步组件和Suspense(真实开发中）
Kuboard can't send email and nail alarm problem is solved
Chinese and English instructions prosci LAG-3 recombinant protein
Special behavior of main function in import statement
Leetcode t1165: log analysis
A slow SQL drags the whole system down
弹性布局（一）
弹性布局（二）
About binary cannot express decimals accurately
选择商品属性弹框从底部弹出动画效果
Prime partner of Huawei machine test questions
栈题目：有效括号的嵌套深度
Pass child component to parent component
AVL树的实现
Abnova immunohistochemical service solution
Circulating tumor cells - here comes abnova's solution