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

Motion planner (MoveIt) and Task planner (Plansys) integration

Browse source
This commit is contained in:
Roman Andrianov 2023-04-30 11:46:52 +00:00 • committed by Igor Brylyov
parent 0a735b87c9
commit 000ddb4831
43 changed files with 1402 additions and 379 deletions
Download patch file Download diff file Expand all files Collapse all files

11
rbs_bt_executor/CMakeLists.txt
View file

@ -5,7 +5,6 @@ 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(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
@ -19,6 +18,7 @@ find_package(ament_index_cpp REQUIRED)
find_package(rbs_skill_interfaces REQUIRED)
find_package(behavior_tree REQUIRED)
find_package(control_msgs REQUIRED)
find_package(component_interfaces REQUIRED)
if (NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
@ -38,6 +38,7 @@ set(dependencies
behavior_tree
std_msgs
control_msgs
component_interfaces
)
include_directories(include)
@ -57,13 +58,17 @@ list(APPEND plugin_libs rbs_skill_move_joint_state)
add_library(rbs_add_planning_scene_object SHARED src/AddPlanningSceneObject.cpp)
list(APPEND plugin_libs rbs_add_planning_scene_object)
add_library(rbs_assemble_process_state SHARED src/AssembleProcessState.cpp)
list(APPEND plugin_libs rbs_assemble_process_state)
add_library(rbs_detect_object SHARED src/DetectObject.cpp)
list(APPEND plugin_libs rbs_detect_object)
foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
endforeach()
install(DIRECTORY launch bt_trees config DESTINATION share/${PROJECT_NAME})
install(TARGETS

57
rbs_bt_executor/bt_trees/assemble.xml Normal file
View file

@ -0,0 +1,57 @@
<?xml version="1.0"?>
<root main_tree_to_execute="Assemble">
<BehaviorTree ID="Assemble">
<Sequence>
<Action ID="DetectObject" ReqKind="FOLLOW" ObjectName="${arg0}"
server_name="/ground_true/scene_component" server_timeout="1000"/>
<Action ID="AssembleState" StateKind="INITIALIZE" PartName="${arg0}"
AssembleName="${arg2}" WorkspaceName="${arg3}" server_name="/assemble_state"
server_timeout="1000"/>
<Action ID="GetPickPlacePoses" ObjectName="${arg0}" GraspDirection = "y" PlaceDirection = "z"
GraspPose="{GraspPose}" PostGraspPose="{PostGraspPose}" PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}" PlacePose="{PlacePose}" PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}" server_name="/get_pick_place_pose_service" server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PreGraspPose}"
server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{GraspPose}"
server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control"
server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PostGraspPose}"
server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PostPostGraspPose}"
server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PrePlacePose}"
server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PlacePose}"
server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control"
server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PostPlacePose}"
server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
`
<Action ID="AssembleState" StateKind="VALIDATE" PartName="${arg0}"
AssembleName="${arg2}" WorkspaceName="${arg3}" server_name="/assemble_state"
server_timeout="1000"/>
<Action ID="AssembleState" StateKind="COMPLETE" PartName="${arg0}"
AssembleName="${arg2}" WorkspaceName="${arg3}" server_name="/assemble_state"
server_timeout="1000"/>
<Action ID="DetectObject" ReqKind="CANCEL" ObjectName="${arg0}"
server_name="/ground_true/scene_component" server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

1
rbs_bt_executor/package.xml
View file

@ -10,6 +10,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rbs_skill_interfaces</depend>
<depend>component_interfaces</depend>
<depend>behavior_tree</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

67
rbs_bt_executor/src/AssembleProcessState.cpp Normal file
View file

@ -0,0 +1,67 @@
#include <string>
#include <behavior_tree/BtService.hpp>
#include <rclcpp/rclcpp.hpp>
#include "rbs_skill_interfaces/srv/assemble_state.hpp"
using AssembleStateSrv = rbs_skill_interfaces::srv::AssembleState;
class AssembleState : public BtService<AssembleStateSrv>
{
public:
AssembleState(const std::string & name, const BT::NodeConfiguration &config)
: BtService<AssembleStateSrv>(name, config)
{
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
AssembleStateSrv::Request::SharedPtr populate_request()
{
auto request = std::make_shared<AssembleStateSrv::Request>();
auto state_kind = getInput<std::string>("StateKind").value();
request->req_kind = -1;
if (state_kind == "INITIALIZE")
request->req_kind = 0;
else if (state_kind == "VALIDATE")
request->req_kind = 1;
else if (state_kind == "COMPLETE")
request->req_kind = 2;
auto assemble_name = getInput<std::string>("AssembleName").value();
auto part_name = getInput<std::string>("PartName").value();
auto workspace = getInput<std::string>("WorkspaceName").value();
request->assemble_name = assemble_name;
request->part_name = part_name;
request->workspace = workspace;
return request;
}
BT::NodeStatus handle_response(AssembleStateSrv::Response::SharedPtr response)
{
// TODO: return bad status on validate process return false state
return (response->call_status = true)? BT::NodeStatus::SUCCESS: BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>("StateKind"),
BT::InputPort<std::string>("AssembleName"),
BT::InputPort<std::string>("PartName"),
BT::InputPort<std::string>("WorkspaceName")
});
}
private:
std::map<std::string, int> assemble_states_;
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<AssembleState>("AssembleState");
}

51
rbs_bt_executor/src/DetectObject.cpp Normal file
View file

@ -0,0 +1,51 @@
#include <string>
#include <behavior_tree/BtService.hpp>
#include <rclcpp/rclcpp.hpp>
#include <component_interfaces/srv/detect_object.hpp>
using DetectObjectSrv = component_interfaces::srv::DetectObject;
class DetectObject: public BtService<DetectObjectSrv>
{
public:
DetectObject(const std::string &name, const BT::NodeConfiguration &config)
: BtService<DetectObjectSrv>(name, config)
{
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
DetectObjectSrv::Request::SharedPtr populate_request()
{
auto request = std::make_shared<DetectObjectSrv::Request>();
auto req_kind = getInput<std::string>("ReqKind").value();
if (req_kind == "DETECT")
request->req_kind = 0;
else if (req_kind == "FOLLOW")
request->req_kind = 1;
else
request->req_kind = 2;
request->object_name = getInput<std::string>("ObjectName").value();
return request;
}
BT::NodeStatus handle_response(DetectObjectSrv::Response::SharedPtr response)
{
return response->call_status? BT::NodeStatus::SUCCESS: BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>("ReqKind"),
BT::InputPort<std::string>("ObjectName"),
});
}
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<DetectObject>("DetectObject");
}