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

fix plansys2 arg bug

Browse source
This commit is contained in:
Splinter1984 2022-02-11 23:03:38 +08:00
parent 54da56c7ae
commit 5b601bb3a9
6 changed files with 28 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

11
robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml
View file

@ -3,19 +3,22 @@
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Sequence>
<Action ID="GetEntityState" PartName="rasmt" server_name="/get_entity_state" server_timeout="10"/>
<Action ID="MoveToPose" arm_group="rasmt_arm_group" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveGripper" arm_group="rasmt_hand_arm_group" server_name="/move_to_joint_states" server_timeout="10" cancel_timeout="5" />
<Action ID="GetEntityState" part_name="${arg1}" server_name="/get_entity_state" server_timeout="10"/>
<Action ID="MoveToPose" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveGripper" arm_group="${arg0}" server_name="/move_to_joint_states" server_timeout="10" cancel_timeout="5" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="GetEntityState">
<input_port name="PartName"/>
<input_port name="part_name"/>
</Action>
<Action ID="MoveToPose">
<input_port name="arm_group"/>
</Action>
<Action ID="MoveGripper">
<input_port name="arm_group"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

10
robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp
View file

@ -11,13 +11,15 @@ class GetEntityState : public BtService<GetEntityStateSrv>
GetEntityState(const std::string & name, const BT::NodeConfiguration & config)
: BtService<GetEntityStateSrv>(name, config){
//getInput<std::string>("PartName", part_name_);
part_name_ = getInput<std::string>("PartName").value();
RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< part_name_<<"]");
}
GetEntityStateSrv::Request::SharedPtr populate_request() override
{
getInput<std::string>("part_name", part_name_);
//part_name_ = getInput<std::string>("PartName").value();
RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< part_name_<<"]");
//GetEntityStateSrv::Request::SharedPtr request;
auto request = std::make_shared<GetEntityStateSrv::Request>();
request->name = part_name_;
@ -33,7 +35,7 @@ class GetEntityState : public BtService<GetEntityStateSrv>
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("PartName")});
InputPort<std::string>("part_name")});
}
private:
std::string part_name_;

8
robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp
View file

@ -13,15 +13,15 @@ class MoveToJointState : public BtAction<MoveToJointStateAction>
public:
MoveToJointState(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToJointStateAction>(name, config) {
robot_name_ = getInput<std::string>("arm_group").value();
gripper_gap = 0.02;
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
}
MoveToJointStateAction::Goal populate_goal() override
{
getInput<std::string>("arm_group", robot_name_);
//robot_name_ = getInput<std::string>("arm_group").value();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
auto goal = MoveToJointStateAction::Goal();
goal.robot_name = robot_name_;
goal.joints_acceleration_scaling_factor = 0.1;

7
robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp
View file

@ -13,16 +13,17 @@ class MoveToPose : public BtAction<MoveToPoseAction>
public:
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config) {
robot_name_ = getInput<std::string>("arm_group").value();
target_pose_.position.x = 0.1;
target_pose_.position.y = 0.1;
target_pose_.position.z = 0.6;
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
}
MoveToPoseAction::Goal populate_goal() override
{
getInput<std::string>("arm_group", robot_name_);
//robot_name_ = getInput<std::string>("arm_group").value();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
auto goal = MoveToPoseAction::Goal();
goal.robot_name = robot_name_;
goal.end_effector_acceleration = 1.0;

12
robossembler_servers/CMakeLists.txt
View file

@ -41,11 +41,11 @@ set(deps
robossembler_interfaces
)
#
include_directories(include)
# include_directories(include)
install(DIRECTORY include
DESTINATION include
)
# install(DIRECTORY include
# DESTINATION include
# )
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
@ -53,11 +53,11 @@ if(BUILD_TESTING)
endif()
add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp)
target_include_directories(move_to_joint_states_action_server PRIVATE include)
# target_include_directories(move_to_joint_states_action_server PRIVATE include)
ament_target_dependencies(move_to_joint_states_action_server ${deps})
add_executable(move_topose_action_server src/move_topose_action_server.cpp)
target_include_directories(move_topose_action_server PRIVATE include)
# target_include_directories(move_topose_action_server PRIVATE include)
ament_target_dependencies(move_topose_action_server ${deps})
install(

1
robossembler_servers/package.xml
View file

@ -18,6 +18,7 @@
<depend>rclcpp_action</depend>
<depend>geometry_msgs</depend>
<depend>action_msgs</depend>
<depend>robossembler_interfaces</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>