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

add workspace inspector skill

Browse source
This commit is contained in:
Ilya Uraev 2023-12-30 23:42:10 +03:00
parent 4f0c43bee9
commit 9efab819af
9 changed files with 158 additions and 85 deletions
Download patch file Download diff file Expand all files Collapse all files

12
rbs_bt_executor/CMakeLists.txt
View file

@ -9,7 +9,6 @@ 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)
@ -21,6 +20,9 @@ find_package(control_msgs REQUIRED)
find_package(lifecycle_msgs REQUIRED)
find_package(rcl_interfaces REQUIRED)
find_package(env_manager_interfaces REQUIRED)
find_package(rbs_utils REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tf2_ros REQUIRED)
if (NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
@ -30,7 +32,6 @@ set(dependencies
rclcpp
rclcpp_action
geometry_msgs
tf2_geometry_msgs
moveit_msgs
moveit_core
moveit_ros_planning
@ -38,11 +39,13 @@ set(dependencies
ament_index_cpp
rbs_skill_interfaces
behavior_tree
std_msgs
control_msgs
lifecycle_msgs
rcl_interfaces
env_manager_interfaces
rbs_utils
tf2_ros
tf2_eigen
)
include_directories(include)
@ -71,6 +74,9 @@ list(APPEND plugin_libs rbs_pose_estimation)
add_library(rbs_env_manager_starter SHARED src/EnvManager.cpp)
list(APPEND plugin_libs rbs_env_manager_starter)
add_library(rbs_skill_move_topose_array_bt_action_client SHARED src/MoveToPoseArray.cpp)
list(APPEND plugin_libs rbs_skill_move_topose_array_bt_action_client)
foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

64
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -1,16 +1,56 @@
<?xml version="1.0"?>
<root main_tree_to_execute="PoseEstimation">
<BehaviorTree ID="PoseEstimation">
<root main_tree_to_execute="MainTree">
<!-- ////////// -->
<BehaviorTree ID="MainTree">
<Sequence>
<Action ID="PoseEstimation"
ObjectName="!/home/shalenikol/robossembler_ws/src/robossembler-ros2/rbs_perception/config/str_param.json"
ReqKind = "calibrate"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
<Action ID="PoseEstimation"
ObjectName=""
ReqKind = "activate"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
<Action ID="EnvStarter" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"
server_name="/env_manager/start_env" server_timeout="1000" workspace="{workspace}" />
<SubTreePlus ID="WorkspaceInspection" __autoremap="1" goal_pose="{workspace}"
robot_name="ur_manipulator" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="WorkspaceInspection">
<Sequence>
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/move_topose" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="EnvStarter">
<inout_port name="env_class" />
<inout_port name="env_name" />
<inout_port name="server_name" />
<inout_port name="server_timeout" />
<inout_port name="workspace" />
</Action>
<Action ID="MoveToPose">
<inout_port name="cancel_timeout" />
<inout_port name="pose" />
<inout_port name="robot_name" />
<inout_port name="server_name" />
<inout_port name="server_timeout" />
</Action>
<Action ID="MoveToPoseArray">
<inout_port name="cancel_timeout" />
<inout_port name="pose_vec_in" />
<inout_port name="robot_name" />
<inout_port name="server_name" />
<inout_port name="server_timeout" />
</Action>
<SubTree ID="PoseEstimation" />
<SubTree ID="WorkspaceInspection" />
</TreeNodesModel>
<!-- ////////// -->
</root>

9
rbs_bt_executor/bt_trees/test_tree_main.xml
View file

@ -15,7 +15,12 @@
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose"
robot_name="ur_manipulator_gripper"
pose="{PreGraspPose}"
server_name="/move_topose"
server_timeout="10000"
cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" 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="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
@ -152,4 +157,4 @@
<input_port name="pose_name"/>
</Action>
</TreeNodesModel>
</root>
</root>

47
rbs_bt_executor/config/params.yaml
View file

@ -1,33 +1,14 @@
# 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
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
]
bt_engine:
ros__parameters:
bt_file_path: "/home/bill-finger/rbs_ws/src/robossembler-ros2/rbs_bt_executor/bt_trees/test_tree.xml"
plugins: [
"rbs_skill_move_topose_bt_action_client",
"rbs_skill_get_pick_place_pose_service_client",
"rbs_skill_gripper_move_bt_action_client",
"rbs_skill_move_joint_state",
"rbs_add_planning_scene_object",
"rbs_pose_estimation",
"rbs_env_manager_starter",
"rbs_skill_move_topose_array_bt_action_client"
]

16
rbs_bt_executor/launch/rbs_executor.launch.py
View file

@ -49,19 +49,7 @@ def generate_launch_description():
Node(
package='behavior_tree',
executable='bt_engine',
#prefix=['xterm -e gdb -ex run --args'],
parameters=[
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree_env_manager.xml')},
{'plugins':["rbs_skill_move_topose_bt_action_client",
"rbs_skill_get_pick_place_pose_service_client",
"rbs_skill_gripper_move_bt_action_client",
"rbs_skill_move_joint_state",
"rbs_add_planning_scene_object",
"rbs_pose_estimation",
"rbs_env_manager_starter"
]},
# gripperPoints,
robot_description_semantic
]
# prefix=['gdbserver localhost:3000'],
parameters=["/home/bill-finger/rbs_ws/src/robossembler-ros2/rbs_bt_executor/config/params.yaml"]
)
])

2
rbs_bt_executor/package.xml
View file

@ -8,7 +8,7 @@
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rbs_utils</depend>
<depend>rbs_skill_interfaces</depend>
<depend>component_interfaces</depend>
<depend>env_manager_interfaces</depend>

10
rbs_bt_executor/src/EnvManager.cpp
View file

@ -2,9 +2,11 @@
#include <behaviortree_cpp_v3/basic_types.h>
#include <env_manager_interfaces/srv/detail/start_env__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <memory>
#include <rclcpp/logging.hpp>
#include <string>
#include "rbs_utils/rbs_utils.hpp"
#include "env_manager_interfaces/srv/start_env.hpp"
#include "env_manager_interfaces/srv/unload_env.hpp"
@ -31,6 +33,11 @@ public:
BT::NodeStatus handle_response(
const EnvStarterService::Response::SharedPtr response) override {
if (response->ok) {
// TODO We need better perfomance for call it in another place for all BT nodes
// - Make it via shared_ptr forward throught blackboard
auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2", _node);
auto p = t->getWorkspaceInspectorTrajectory();
setOutput("workspace", p);
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
@ -40,6 +47,7 @@ public:
return providedBasicPorts({
BT::InputPort<std::string>("env_name"),
BT::InputPort<std::string>("env_class"),
BT::OutputPort<geometry_msgs::msg::PoseArray>("workspace"),
});
}
};

27
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -1,19 +1,14 @@
// Copyright [2023] bill-finger
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#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:
public:
MoveToPose(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToPoseAction>(name, config) {}
@ -42,23 +37,17 @@ public:
return goal;
}
static PortsList providedPorts() {
return providedBasicPorts({InputPort<std::string>("robot_name"),
InputPort<geometry_msgs::msg::Pose>("pose")});
static BT::PortsList providedPorts() {
return providedBasicPorts(
{BT::InputPort<std::string>("robot_name"),
BT::InputPort<geometry_msgs::msg::Pose>("pose")});
}
private:
std::string robot_name_;
// std::vector<double> pose_;
geometry_msgs::msg::Pose pose_des;
// std::map<std::string, geometry_msgs::msg::Pose> Poses;
// geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){
// }
}; // class MoveToPose
}; // class MoveToPose
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToPose>("MoveToPose");
}
}

56
rbs_bt_executor/src/MoveToPoseArray.cpp Normal file
View file

@ -0,0 +1,56 @@
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "geometry_msgs/msg/pose_array.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_utils/rbs_utils.hpp"
#include <behaviortree_cpp_v3/bt_factory.h>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node.hpp>
using MoveToPoseArrayAction = rbs_skill_interfaces::action::MoveitSendPose;
class MoveToPoseArray : public BtAction<MoveToPoseArrayAction> {
public:
MoveToPoseArray(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToPoseArrayAction>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
MoveToPoseArrayAction::Goal populate_goal() override {
auto goal = MoveToPoseArrayAction::Goal();
getInput<std::string>("robot_name", robot_name_);
getInput<geometry_msgs::msg::PoseArray>("pose_vec_in", target_pose_vec_);
for (auto &point : target_pose_vec_.poses) {
RCLCPP_INFO(_node->get_logger(), "Pose array: \n %f \n %f \n %f",
point.position.x, point.position.y, point.position.z);
}
if (!target_pose_vec_.poses.empty()) {
goal.robot_name = robot_name_;
goal.target_pose = target_pose_vec_.poses.at(0);
target_pose_vec_.poses.erase(target_pose_vec_.poses.begin());
setOutput<geometry_msgs::msg::PoseArray>("pose_vec_out", target_pose_vec_);
} else {
RCLCPP_WARN(_node->get_logger(), "Target pose vector empty");
}
return goal;
}
static BT::PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("robot_name"),
BT::InputPort<geometry_msgs::msg::PoseArray>("pose_vec_in"),
BT::OutputPort<geometry_msgs::msg::PoseArray>("pose_vec_out")
});
}
private:
std::string robot_name_;
geometry_msgs::msg::PoseArray target_pose_vec_;
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToPoseArray>("MoveToPoseArray");
}