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

rbs_utils in env_manager and clear code

Browse source
This commit is contained in:
Ilya Uraev 2023-12-14 12:00:35 +03:00
parent 0588a5f6c6
commit 34c8961723
23 changed files with 536 additions and 237 deletions
Download patch file Download diff file Expand all files Collapse all files

14
rbs_bt_executor/bt_trees/test_gripper.xml Normal file
View file

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence>
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
</Sequence>
</BehaviorTree>
</root>

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

@ -51,7 +51,7 @@ def generate_launch_description():
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.xml')},
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_gripper.xml')},
{'plugins':["rbs_skill_move_topose_bt_action_client",
"rbs_skill_get_pick_place_pose_service_client",
"rbs_skill_gripper_move_bt_action_client",

4
rbs_bt_executor/src/MoveToJointStates.cpp
View file

@ -20,7 +20,7 @@ public:
MoveToJointStateAction::Goal populate_goal() override {
getInput<std::string>("robot_name", robot_name_);
getInput<std::vector<double>>("PrePlaceJointState", joint_values_);
getInput<std::vector<double>>("JointState", joint_values_);
auto goal = MoveToJointStateAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal for robot [%s]",
@ -42,7 +42,7 @@ public:
static PortsList providedPorts() {
return providedBasicPorts(
{InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("PrePlaceJointState")});
InputPort<std::vector<double>>("JointState")});
}
private:

8
rbs_bt_executor/src/gripper_move.cpp
View file

@ -19,9 +19,6 @@ public:
if (!_node->has_parameter("close")) {
_node->declare_parameter("close", position.close);
}
if (!_node->has_parameter("midPosition")) {
_node->declare_parameter("midPosition", position.closeFull);
}
}
GripperCommand::Goal populate_goal() override {
getInput<std::string>("pose", pose);
@ -44,9 +41,8 @@ public:
private:
struct {
double open = 0.2;
double closeFull = 0.8;
double close = 0.4;
double open = 0.008;
double close = 0.000;
} position;
std::string pose;