robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

Add Plansys2 for task planning

Browse source
This commit is contained in:
Roman Andrianov 2023-04-06 18:27:26 +00:00 • committed by Igor Brylyov
parent a87fb8a7ec
commit d61cd85b3c
12 changed files with 578 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

58
env_components/assemble_component/CMakeLists.txt Normal file
View file

@ -0,0 +1,58 @@
cmake_minimum_required(VERSION 3.5)
project(assemble_component)
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED True)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_ros REQUIRED)
find_package(rbs_skill_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(env_manager REQUIRED)
add_library(assemble_state_component SHARED
src/assemble_state_component.cpp)
target_compile_definitions(assemble_state_component
PRIVATE "COMPOSITION_BUILDING_DLL")
ament_target_dependencies(assemble_state_component
"rbs_skill_interfaces"
"rclcpp"
"rclcpp_components"
"tf2_ros"
"tf2_msgs"
"geometry_msgs"
"env_manager")
rclcpp_components_register_nodes(assemble_state_component "assemble_component::AssembleStateComponent")
set(node_plugins "${node_plugins}assemble_component::AssembleStateComponent;$<TARGET_FILE:assemble_component>\n")
install(TARGETS assemble_state_component
DESTINATION lib)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

26
env_components/assemble_component/package.xml Normal file
View file

@ -0,0 +1,26 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>assemble_component</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>pkg-config</buildtool_depend>
<depend>rbs_skill_interfaces</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>env_manager</depend>
<depend>tf2_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

233
env_components/assemble_component/src/assemble_state_component.cpp Normal file
View file

@ -0,0 +1,233 @@
#include <component_manager/service_component.hpp>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_msgs/msg/tf_message.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rbs_skill_interfaces/srv/assemble_state.hpp>
namespace assemble_component
{
const std::string ASSEMBLE_PACKAGE_NAME_PARAM = "assemble_package_name";
const std::string ASSEMBLE_PREFIX_NAME = "ASSEMBLE";
const double ASSEMBLE_POSITION_OFFSET = 0.5;
const double ASSEMBLE_ORIENTATION_OFFSET = 0.5;
/* TODO: implement sdf parse for assemble_solver.
* */
struct assemble_solver
{
std::string package_name;
std::string assemble_name;
std::string part_name;
std::string workspace;
std::map<std::string, geometry_msgs::msg::PoseStamped> assemble_parts;
};
class AssembleStateComponent: public env_manager::component_manager
::ServiceComponent<rbs_skill_interfaces::srv::AssembleState>
{
public:
AssembleStateComponent(const rclcpp::NodeOptions& options)
: env_manager::component_manager
::ServiceComponent<rbs_skill_interfaces::srv::AssembleState>(options)
{
this->declare_parameter(ASSEMBLE_PACKAGE_NAME_PARAM, NULL);
param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
auto pevcallback = [this](const rclcpp::Parameter & p) {
if (ASSEMBLE_PACKAGE_NAME_PARAM == p.get_name() &&
assemble_package_name_ != p.as_string() &&
current_state_ == AssembleReqState::NONE)
assemble_package_name_ = p.as_string();
else
RCLCPP_WARN(get_logger(),
"Unable to change param while assemble process in progress.");
};
cb_handle_ = param_subscriber_->add_parameter_callback(
ASSEMBLE_PACKAGE_NAME_PARAM, pevcallback);
tf_buffer_ =
std::make_unique<tf2_ros::Buffer>(get_clock());
tf_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
}
void callback(
std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Request> request,
std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Response> response)
{
auto state = static_cast<AssembleReqState>(request->req_kind);
auto call_status = false;
switch(state)
{
case AssembleReqState::NONE:
response->call_status = false;
return;
case AssembleReqState::INITIALIZE:
call_status = (current_state_ != AssembleReqState::NONE)? false:
on_initialize(request->assemble_name, request->part_name, request->workspace);
if (!call_status)
RCLCPP_WARN_STREAM(get_logger(), "callback: " << "initialize error.");
break;
case AssembleReqState::VALIDATE:
call_status = (current_state_ == AssembleReqState::INITIALIZE);
response->validate_status = (call_status)? on_validate(): call_status;
if (!call_status)
RCLCPP_WARN_STREAM(get_logger(), "callback: " << "validate error");
break;
case AssembleReqState::COMPLETE:
call_status = (current_state_ != AssembleReqState::VALIDATE)? false: on_complete();
if (!call_status)
RCLCPP_WARN_STREAM(get_logger(), "callback: " << "complete process error.");
break;
case AssembleReqState::CANCEL:
call_status = (current_state_ == AssembleReqState::NONE)? false: on_cancel();
if (!call_status)
RCLCPP_WARN_STREAM(get_logger(), "callback: " << "complete process error.");
break;
}
response->call_status = call_status;
}
bool on_initialize(
const std::string &assemble_name,
const std::string &part_name,
const std::string &workspace)
{
if (current_state_ != AssembleReqState::NONE)
{
RCLCPP_WARN_STREAM(get_logger(), "on_initialize: " << "assemble in progress");
return false;
}
try {
mt.lock();
assemble_ = (assemble_solver){
.package_name=assemble_package_name_,
.assemble_name=assemble_name,
.part_name=part_name,
.workspace=workspace};
mt.unlock();
} catch (std::exception &ex) {
RCLCPP_ERROR_STREAM(get_logger(), "on_initialize: " << ex.what());
return false;
}
current_state_ = AssembleReqState::INITIALIZE;
tf_broadcaster_ =
std::make_unique<tf2_ros::StaticTransformBroadcaster>(this);
send_transform(assemble_);
return true;
}
bool on_validate()
{
std::string frame_from = ASSEMBLE_PREFIX_NAME + assemble_.part_name;
std::string frame_to = assemble_.part_name;
geometry_msgs::msg::TransformStamped t;
try {
t = tf_buffer_->lookupTransform(
frame_to, frame_from, tf2::TimePointZero);
} catch (const tf2::TransformException &ex) {
RCLCPP_WARN_STREAM(get_logger(),
"Could not transform "
<< frame_to << " to " << frame_from
<< " " << ex.what());
return false;
}
auto pos_validate =
t.transform.translation.x < ASSEMBLE_POSITION_OFFSET &&
t.transform.translation.y < ASSEMBLE_POSITION_OFFSET &&
t.transform.translation.z < ASSEMBLE_POSITION_OFFSET;
auto orient_validate =
t.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET &&
t.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET &&
t.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET &&
t.transform.rotation.w < ASSEMBLE_ORIENTATION_OFFSET;
return pos_validate && orient_validate;
}
bool on_complete()
{
try {
tf_broadcaster_.reset();
} catch (const std::exception &ex) {
RCLCPP_ERROR_STREAM(get_logger(), "on_complete: " << ex.what());
return false;
}
current_state_ = AssembleReqState::NONE;
assemble_ = {};
return true;
}
bool on_cancel()
{
return on_complete();
}
protected:
enum AssembleReqState
{
NONE = -1,
INITIALIZE = 0,
VALIDATE = 1,
COMPLETE = 2,
CANCEL = 3
};
private:
void send_transform(const assemble_solver &solver)
{
if (solver.assemble_parts.empty())
{
RCLCPP_WARN_STREAM(
get_logger(), "send_transform: " << "solver parts are empty");
return;
}
for (const auto& pt: solver.assemble_parts)
{
geometry_msgs::msg::TransformStamped t;
t.header.frame_id = solver.workspace;
t.child_frame_id = pt.first.c_str();
auto pose = pt.second.pose;
t.transform.translation.x = pose.position.x;
t.transform.translation.y = pose.position.y;
t.transform.translation.z = pose.position.z;
t.transform.rotation.x = pose.orientation.x;
t.transform.rotation.y = pose.orientation.y;
t.transform.rotation.z = pose.orientation.z;
t.transform.rotation.w = pose.orientation.w;
tf_broadcaster_->sendTransform(t);
}
}
std::string assemble_package_name_;
AssembleReqState current_state_;
std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_handle_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf_broadcaster_;
assemble_solver assemble_;
std::mutex mt;
};
} // namespace assemble_component
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(assemble_component::AssembleStateComponent)

4
rbs_skill_interfaces/CMakeLists.txt
View file

@ -21,9 +21,11 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/PropertyValuePair.msg"
"msg/ActionFeedbackStatusConstants.msg"
"msg/ActionResultStatusConstants.msg"
"srv/BtInit.srv"
"srv/AssembleState.srv"
"srv/GetPickPlacePoses.srv"
"srv/AddPlanningSceneObject.srv"
DEPENDENCIES geometry_msgs std_msgs
DEPENDENCIES std_msgs geometry_msgs
)

3
rbs_skill_interfaces/package.xml
View file

@ -12,6 +12,9 @@
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<depend>std_msgs</depend>
<depend>geometry_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

31
rbs_skill_interfaces/srv/AssembleState.srv Normal file
View file

@ -0,0 +1,31 @@
# we can call different state for assemble process
# INITIALIZE - find all information about assemble and provide it to tf2
uint8 INITIALIZE=0
# VALIDATE - check current state with correct assemle state
uint8 VALIDATE=1
# COMPLETE - finalize assemble process
uint8 COMPLETE=2
# CANCEL - force finalize assemble process
uint8 CANCEL=3
string assemble_name
string part_name
uint8 req_kind
# workspace used only for INITIALIZE req_kind
string workspace
---
bool call_status
# on error
string error_msg
# validate_status variable needed only for VALIDATE call
bool validate_status

46
rbs_task_planner/CMakeLists.txt Normal file
View file

@ -0,0 +1,46 @@
cmake_minimum_required(VERSION 3.5)
project(rbs_task_planner)
find_package(ament_cmake REQUIRED)
find_package(plansys2_msgs REQUIRED)
find_package(plansys2_domain_expert REQUIRED)
find_package(plansys2_executor REQUIRED)
find_package(plansys2_planner REQUIRED)
find_package(plansys2_problem_expert REQUIRED)
find_package(plansys2_pddl_parser REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(plansys2_bt_actions REQUIRED)
find_package(behavior_tree REQUIRED)
find_package(test_msgs REQUIRED)
if (NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
set(dependencies
plansys2_msgs
plansys2_domain_expert
plansys2_executor
plansys2_planner
plansys2_problem_expert
plansys2_pddl_parser
ament_index_cpp
plansys2_bt_actions
behavior_tree
test_msgs
)
install(DIRECTORY launch domain problems config DESTINATION share/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_gtest REQUIRED)
endif()
ament_export_dependencies(${dependencies})
ament_package()

0
rbs_task_planner/config/params.yaml Normal file
View file

34
rbs_task_planner/domain/atomic_domain.pddl Normal file
View file

@ -0,0 +1,34 @@
(define (domain atomic_domain)
(:requirements :strips :typing :adl :fluents :durative-actions)
(:types
workspace - zone
part
arm
assembly
)
(:predicates
(arm_available ?a - arm)
(assembly_at ?a - assembly ?z - zone)
(part_of ?part - part ?whole - assembly)
(assembled ?whole - assembly)
(assembly_order ?prev ?next - assembly)
)
(:durative-action assemble
:parameters (?p - part ?prev ?next - assembly ?z - zone ?a - arm)
:duration ( = ?duration 1)
:condition (and
(at start (assembly_order ?prev ?next))
(at start (assembled ?prev))
(at start (assembly_at ?prev ?z))
(at start (part_of ?p ?next))
(at start (arm_available ?a))
)
:effect (and
(at start (not (arm_available ?a)))
(at end (assembly_at ?next ?z))
(at end (assembled ?next))
)
)
)

80
rbs_task_planner/launch/task_planner.launch.py Normal file
View file

@ -0,0 +1,80 @@
import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
# from launch_ros.actions import Node
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError:
# parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError:
# parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
pkg_dir = get_package_share_directory('rbs_task_planner')
namespace = LaunchConfiguration('namespace')
declare_namespace_cmd = DeclareLaunchArgument(
name="namespace",
default_value='',
description='Namespace')
plansys2_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(
get_package_share_directory('plansys2_bringup'),
'launch',
'plansys2_bringup_launch_monolithic.py')),
launch_arguments={
'model_file': pkg_dir + '/domain/atomic_domain.pddl',
'namespace': namespace
}.items())
"""
assemble = Node(
package='plansys2_bt_actions',
executable='bt_action_node',
name='assemble',
namespace=namespace,
output='screen',
parameters=[
pkg_dir + '/config/params.yaml',
robot_description,
robot_description_semantic,
kinematics_yaml,
{
'action_name': 'assemble',
# 'publisher_port': 1666,
# 'server_port': 1667,
'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/assemble.xml',
}
])
"""
ld = LaunchDescription()
ld.add_action(declare_namespace_cmd)
# Declare the launch options
ld.add_action(plansys2_cmd)
return ld

34
rbs_task_planner/package.xml Normal file
View file

@ -0,0 +1,34 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rbs_task_planner</name>
<version>0.0.1</version>
<description>ROS2 task planner for manipulator</description>
<maintainer email="rom.andrianov1984@gmail.com">Splinter1984</maintainer>
<license>Apache License, Version 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>ament_index_cpp</depend>
<depend>plansys2_pddl_parser</depend>
<depend>plansys2_msgs</depend>
<depend>plansys2_domain_expert</depend>
<depend>plansys2_problem_expert</depend>
<depend>plansys2_planner</depend>
<depend>plansys2_executor</depend>
<depend>plansys2_bt_actions</depend>
<depend>behaviortree_cpp_v3</depend>
<exec_depend>popf</exec_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>test_msgs</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

30
rbs_task_planner/problems/problem.pddl Normal file
View file

@ -0,0 +1,30 @@
(define (problem cube3-problem)
(:domain atomic_domain)
(:objects
ws1 - zone
rbsarm - arm
cube3 cube1 cube2 cube004 cube005 cube006 cube007 - part
zeroasm subasm0 subasm1 subasm2 subasm3 subasm4 subasm5 subasm6 - assembly
)
(:init
(part_of cube3 subasm0)
(part_of cube1 subasm1)
(part_of cube2 subasm2)
(part_of cube004 subasm3)
(part_of cube005 subasm4)
(part_of cube006 subasm5)
(part_of cube007 subasm6)
(assembly_order zeroasm subasm0)
(assembly_order subasm0 subasm1)
(assembly_order subasm1 subasm2)
(assembly_order subasm3 subasm4)
(assembly_order subasm5 subasm6)
(arm_available rbsarm)
(assembly_at zeroasm ws1)
(assembled zeroasm)
)
(:goal (and (assembled subasm6)))
)