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

Migrate to Gazebo Fortress

Browse source
This commit is contained in:
Ilya Uraev 2023-02-03 07:04:12 +00:00 • committed by Igor Brylyov
parent b34c00a9b9
commit e46c7bef74
113 changed files with 2751 additions and 25450 deletions
Download patch file Download diff file Expand all files Collapse all files

80
rbs_bt_executor/CMakeLists.txt Normal file
View file

@ -0,0 +1,80 @@
cmake_minimum_required(VERSION 3.8)
project(rbs_bt_executor)
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)
find_package(geometry_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(rbs_skill_interfaces REQUIRED)
find_package(behavior_tree REQUIRED)
find_package(control_msgs REQUIRED)
if (NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
set(dependencies
rclcpp
rclcpp_action
geometry_msgs
tf2_geometry_msgs
moveit_msgs
moveit_core
moveit_ros_planning
moveit_ros_planning_interface
ament_index_cpp
rbs_skill_interfaces
behavior_tree
std_msgs
control_msgs
)
include_directories(include)
add_library(rbs_skill_move_topose_bt_action_client SHARED src/MoveToPose.cpp)
list(APPEND plugin_libs rbs_skill_move_topose_bt_action_client)
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
${plugin_libs}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
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_export_dependencies(${dependencies})
ament_package()

34
rbs_bt_executor/bt_trees/atomic_skills_xml/assemble.xml Normal file
View file

@ -0,0 +1,34 @@
<?xml version="1.0"?>
<root main_tree_to_execute="Assemble">
<!-- ////////// -->
<BehaviorTree ID="Assemble">
<Sequence>
<Action ID="GetGraspPoses" frame="${arg0}" server_name="/scene_monitor/get_grasp_poses" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" command="go_to" arm_group="${arg4}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperCmd" command="open" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" command="down" arm_group="${arg4}" server_name="/move_cartesian" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperCmd" command="close" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" command="up" arm_group="${arg4}" server_name="/move_cartesian" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperCmd" command="open" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="GetGraspPoses">
<input_port name="frame"/>
<output_port name="grasp_pose"/>
</Action>
<Action ID="MoveToPose">
<input_port name="arm_group"/>
<input_port name="command"/>
</Action>
<Action ID="MoveGripper">
<input_port name="arm_group"/>
<input_port name="command"/>
</Action>
<Action ID="Print">
<input_port name="frame"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

40
rbs_bt_executor/bt_trees/atomic_skills_xml/get-part-state.xml Normal file
View file

@ -0,0 +1,40 @@
<?xml version="1.0"?>
<root main_tree_to_execute="GraspPart">
<!-- ////////// -->
<BehaviorTree ID="GraspPart">
<Fallback name="root_Fallback">
<Sequence name="GraspSkill">
<Fallback>
<Condition ID="PartInScene"/>
<Action ID="FindPart" PartName="{arg1}" PartPose="{arg0}"/>
</Fallback>
<Action ID="MoveToPoint" arm_group="${arg2}" goal="${arg0}"/>
<Action ID="GraspPart" arm_group="${arg3}" fingers_wide="${arg4}"/>
<Action ID="MoveToPoint" arm_group="{arg2}" goal="{arg5}"/>
</Sequence>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="FindPart">
<inout_port name="PartName"/>
<output_port name="PartPose"/>
</Action>
<Action ID="GraspPart">
<input_port name="arm_group"/>
<input_port name="fingers_wide"/>
</Action>
<Action ID="MoveToPoint">
<input_port name="arm_group"/>
<input_port name="goal"/>
</Action>
<Condition ID="PartInScene"/>
<Condition ID="PathFree"/>
<Action ID="SpawnPart">
<inout_port name="PartName"/>
<input_port name="PartPose"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

42
rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_part.xml Normal file
View file

@ -0,0 +1,42 @@
<?xml version="1.0"?>
<root main_tree_to_execute="GraspPart">
<!-- ////////// -->
<BehaviorTree ID="GraspPart">
<Fallback name="root_Fallback">
<Sequence name="GraspSkill">
<ReactiveFallback name="PartEstimate">
<Condition ID="PartInScene"/>
<Action ID="FindPart" PartName="{arg1}" PartPose="{arg0}"/>
</ReactiveFallback>
<Fallback name="Move">
<Condition ID="PathFree"/>
<Action ID="MoveToPoint" arm_group="${arg2}" goal="${arg0}"/>
</Fallback>
<Action ID="GraspPart" arm_group="${arg3}"/>
<Action ID="MoveToPoint" arm_group="{arg2}" goal="{arg4}"/>
</Sequence>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="FindPart">
<inout_port name="PartName"/>
<output_port name="PartPose"/>
</Action>
<Action ID="GraspPart">
<input_port name="arm_group"/>
</Action>
<Action ID="MoveToPoint">
<input_port name="arm_group"/>
<input_port name="goal"/>
</Action>
<Condition ID="PartInScene"/>
<Condition ID="PathFree"/>
<Action ID="SpawnPart">
<inout_port name="PartName"/>
<input_port name="PartPose"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

38
rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_skill.xml Normal file
View file

@ -0,0 +1,38 @@
<?xml version="1.0"?>
<root main_tree_to_execute="GraspPart">
<!-- ////////// -->
<BehaviorTree ID="GraspPart">
<Sequence name="GraspSkill">
<Action ID="SpawnPart" PartName="${arg2}" PartPose="${gazebo_entity_name}"/>
<Sequence>
<Condition ID="PartInScene"/>
<Action ID="FindPart" PartName="${arg2}" PartPose="${part_pose}"/>
</Sequence>
<Action ID="Move" arm_group="${arg0}" goal="${part_pose}"/>
<Action ID="GraspPart" arm_group="${arg1}" fingers_wide="${goal1}"/>
<Action ID="Move" arm_group="${arg0}" goal="${goal2}"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="FindPart">
<inout_port name="PartName"/>
<output_port name="PartPose"/>
</Action>
<Action ID="GraspPart">
<input_port name="arm_group"/>
<input_port name="fingers_wide"/>
</Action>
<Action ID="Move">
<input_port name="arm_group"/>
<input_port name="goal"/>
</Action>
<Condition ID="PartInScene"/>
<Condition ID="PathFree"/>
<Action ID="SpawnPart">
<inout_port name="PartName"/>
<input_port name="PartPose"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

7
rbs_bt_executor/bt_trees/atomic_skills_xml/move.xml Normal file
View file

@ -0,0 +1,7 @@
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<Move name="move" arm_group="${arg0}" goal="${arg1}"/>
</Sequence>
</BehaviorTree>
</root>

7
rbs_bt_executor/bt_trees/atomic_skills_xml/move_gripper.xml Normal file
View file

@ -0,0 +1,7 @@
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<MoveGripper name="move_gripper" gripper_group="${arg0}" relative_gap="${arg1}"/>
</Sequence>
</BehaviorTree>
</root>

17
rbs_bt_executor/bt_trees/atomic_skills_xml/print.xml Normal file
View file

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<root main_tree_to_execute="Print">
<!-- ////////// -->
<BehaviorTree ID="Print">
<Sequence>
<Action ID="Print" frame="${arg0}" printer="${arg1}" server_name="/scene_monitor/print_part" server_timeout="10" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="Print">
<input_port name="frame"/>
<input_port name="printer"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

144
rbs_bt_executor/bt_trees/atomic_skills_xml/remove.xml Normal file
View file

@ -0,0 +1,144 @@
<?xml version="1.0"?>
<root main_tree_to_execute="Remove">
<!-- ////////// -->
<BehaviorTree ID="Remove">
<Sequence>
<Action ID="GetGraspPoses"
part_id="${arg0}"
grasp_pose="{grasp_pose}"
pre_grasp_pose="{pre_grasp_pose}"
pre_gap_distance="{pre_gap_distance}"
gap_distance="{gap_distance}"
server_name="/scene_monitor/get_grasp_poses"
server_timeout="10" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{pre_grasp_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="GripperCmd"
gap_distance="{pre_gap_distance}"
frame_name="${arg0}"
server_name="/gripper_cmd"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{grasp_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="GripperCmd"
gap_distance="{gap_distance}"
frame_name="${arg0}"
server_name="/gripper_cmd"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{pre_grasp_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="ConstructWorkspacePlacementPose"
part_id="${arg0}"
workspace="${arg2}"
constructed_part="constructed_part"
server_name="/scene_monitor/construct_workspace_placement_pose"
server_timeout="10"/>
<Action ID="GetWorkspacePlacementPose"
part_id="{constructed_part}"
workspace="${arg2}"
pre_placement_pose="{pre_placement_pose}"
placement_pose="{placement_pose}"
server_name="/scene_monitor/get_workspace_placement_pose"
server_timeout="10" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{pre_placement_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{placement_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="GripperCmd"
gap_distance="{pre_gap_distance}"
frame_name="${arg0}"
server_name="/gripper_cmd"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{pre_placement_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="GetGraspPoses">
<input_port name="part_id"/>
<output_port name="grasp_pose"/>
<output_port name="pre_grasp_pose"/>
<output_port name="pre_gap_distance"/>
<output_port name="gap_distance"/>
</Action>
<Action ID="MoveToPose">
<input_port name="robot_name"/>
<input_port name="pose"/>
</Action>
<Action ID="GripperCmd">
<input_port name="gap_distance"/>
<input_port name="frame_name"/>
</Action>
<Action ID="ConstructWorkspacePlacementPose">
<input_port name="part_id"/>
<input_port name="workspace"/>
<output_port name="constructed_part"/>
</Action>
<Action ID="GetWorkspacePlacementPose">
<input_port name="part_id"/>
<input_port name="workspace"/>
<output_port name="pre_placement_pose"/>
<output_port name="placement_pose"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

37
rbs_bt_executor/bt_trees/test_tree.xml Normal file
View file

@ -0,0 +1,37 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence>
<Action ID="MoveToPose"
robot_name="ur_manipulator"
pose="pose1"
server_name="/move_topose"
server_timeout="1000"
cancel_timeout="500" />
<!-- <Action ID="MoveToPose"
robot_name="ur_manipulator"
pose="pose2"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose"
robot_name="ur_manipulator"
pose="pose3"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" /> -->
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID="MoveToPose">
<input_port name="robot_name"/>
<input_port name="pose"/>
</Action>
</TreeNodesModel>
</root>

6
rbs_bt_executor/config/params.yaml Normal file
View file

@ -0,0 +1,6 @@
# Here is a nodes which calling from launch for bt_tree
simple_move_bt_nodes:
ros__parameters:
plugins:
- rbs_skill_move_topose_bt_action_client

29
rbs_bt_executor/config/points.yaml Normal file
View file

@ -0,0 +1,29 @@
bt_engine:
ros__parameters:
pose1: [
0.11724797630931184, #X position
0.46766635768602544, #Y
0.5793862343094913, #Z
0.9987999001314066, #X orientation
0.041553846820940925, #Y
-0.004693314677006583, #Z
-0.025495295825239704 #W
]
pose2: [
-0.11661364861606047,
0.4492600889665261,
0.5591700913807053,
0.9962273179258467,
0.04057380155886888,
0.009225849745372298,
0.07615629548377048
]
pose3: [
-0.07133612047767886,
0.41038906578748613,
0.2844649846989331,
0.999078481789772,
0.04109234110437432,
0.006539754292177074,
0.010527978961032304
]

35
rbs_bt_executor/launch/rbs_executor.launch.py Normal file
View file

@ -0,0 +1,35 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
bt_config = os.path.join(
get_package_share_directory('rbs_bt_executor'),
'config',
'params.yaml'
)
points = os.path.join(
get_package_share_directory('rbs_bt_executor'),
'config',
'points.yaml'
)
return LaunchDescription([
Node(
package='behavior_tree',
namespace='',
executable='bt_engine',
name="bt_engine",
# Do not declare a node name otherwise it messes with the action node names and will result in
# duplicate nodes!
parameters=[
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_queue'), 'bt_xmls/test_tree.xml')},
{'plugins': ['rbs_skill_move_topose_bt_action_client']},
points
]
),
])

20
rbs_bt_executor/package.xml Normal file
View file

@ -0,0 +1,20 @@
<?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_bt_executor</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ur.narmak@gmail.com">root</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rbs_skill_interfaces</depend>
<depend>behavior_tree</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

75
rbs_bt_executor/src/MoveToPose.cpp Normal file
View file

@ -0,0 +1,75 @@
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
// #include "Eigen/Geometry"
#include "rclcpp/parameter.hpp"
using namespace BT;
using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPoseActionClient");
class MoveToPose : public BtAction<MoveToPoseAction>
{
public:
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config)
{
//auto params = _node->declare_parameters("Poses")
RCLCPP_INFO(_node->get_logger(), "Start the node");
pose_des.position.x = 0.11724797630931184;
pose_des.position.y = 0.46766635768602544;
pose_des.position.z = 0.5793862343094913;
pose_des.orientation.x = 0.9987999001314066;
pose_des.orientation.y = 0.041553846820940925;
pose_des.orientation.z = -0.004693314677006583;
pose_des.orientation.w = -0.025495295825239704;
}
MoveToPoseAction::Goal populate_goal() override
{
getInput<std::string>("robot_name", robot_name_);
getInput<std::string>("pose", pose_);
//auto pose = _node->get_parameter(pose_).get_parameter_value().get<geometry_msgs::msg::Pose>();
RCLCPP_INFO(_node->get_logger(), "GrasPose pose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f",
pose_des.position.x, pose_des.position.y, pose_des.position.z,
pose_des.orientation.x, pose_des.orientation.y, pose_des.orientation.z, pose_des.orientation.w);
auto goal = MoveToPoseAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
goal.robot_name = robot_name_;
goal.target_pose = pose_des;
goal.end_effector_acceleration = 1.0;
goal.end_effector_velocity = 1.0;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
return goal;
}
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<std::string>("pose")
});
}
private:
std::string robot_name_;
std::string pose_;
geometry_msgs::msg::Pose pose_des;
std::map<std::string, geometry_msgs::msg::Pose> Poses;
}; // class MoveToPose
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<MoveToPose>("MoveToPose");
}