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"> <BehaviorTree ID="BehaviorTree">
<Sequence> <Sequence>
<Action ID="GetEntityState" PartName="rasmt" server_name="/get_entity_state" server_timeout="10"/> <Action ID="GetEntityState" part_name="${arg1}" 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="MoveToPose" arm_group="${arg0}" 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="MoveGripper" arm_group="${arg0}" server_name="/move_to_joint_states" server_timeout="10" cancel_timeout="5" />
</Sequence> </Sequence>
</BehaviorTree> </BehaviorTree>
<!-- ////////// --> <!-- ////////// -->
<TreeNodesModel> <TreeNodesModel>
<Action ID="GetEntityState"> <Action ID="GetEntityState">
<input_port name="PartName"/> <input_port name="part_name"/>
</Action> </Action>
<Action ID="MoveToPose"> <Action ID="MoveToPose">
<input_port name="arm_group"/> <input_port name="arm_group"/>
</Action> </Action>
<Action ID="MoveGripper">
<input_port name="arm_group"/>
</Action>
</TreeNodesModel> </TreeNodesModel>
<!-- ////////// --> <!-- ////////// -->
</root> </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) GetEntityState(const std::string & name, const BT::NodeConfiguration & config)
: BtService<GetEntityStateSrv>(name, 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 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; //GetEntityStateSrv::Request::SharedPtr request;
auto request = std::make_shared<GetEntityStateSrv::Request>(); auto request = std::make_shared<GetEntityStateSrv::Request>();
request->name = part_name_; request->name = part_name_;
@ -33,7 +35,7 @@ class GetEntityState : public BtService<GetEntityStateSrv>
static PortsList providedPorts() static PortsList providedPorts()
{ {
return providedBasicPorts({ return providedBasicPorts({
InputPort<std::string>("PartName")}); InputPort<std::string>("part_name")});
} }
private: private:
std::string part_name_; 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: public:
MoveToJointState(const std::string & name, const BT::NodeConfiguration & config) MoveToJointState(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToJointStateAction>(name, config) { : BtAction<MoveToJointStateAction>(name, config) {
robot_name_ = getInput<std::string>("arm_group").value();
gripper_gap = 0.02; gripper_gap = 0.02;
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
} }
MoveToJointStateAction::Goal populate_goal() override 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(); auto goal = MoveToJointStateAction::Goal();
goal.robot_name = robot_name_; goal.robot_name = robot_name_;
goal.joints_acceleration_scaling_factor = 0.1; 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: public:
MoveToPose(const std::string & name, const BT::NodeConfiguration & config) MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config) { : BtAction<MoveToPoseAction>(name, config) {
robot_name_ = getInput<std::string>("arm_group").value();
target_pose_.position.x = 0.1; target_pose_.position.x = 0.1;
target_pose_.position.y = 0.1; target_pose_.position.y = 0.1;
target_pose_.position.z = 0.6; 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 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(); auto goal = MoveToPoseAction::Goal();
goal.robot_name = robot_name_; goal.robot_name = robot_name_;
goal.end_effector_acceleration = 1.0; goal.end_effector_acceleration = 1.0;

12
robossembler_servers/CMakeLists.txt
View file

@ -41,11 +41,11 @@ set(deps
robossembler_interfaces robossembler_interfaces
) )
# #
include_directories(include) # include_directories(include)
install(DIRECTORY include # install(DIRECTORY include
DESTINATION include # DESTINATION include
) # )
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
@ -53,11 +53,11 @@ if(BUILD_TESTING)
endif() endif()
add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp) 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}) ament_target_dependencies(move_to_joint_states_action_server ${deps})
add_executable(move_topose_action_server src/move_topose_action_server.cpp) 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}) ament_target_dependencies(move_topose_action_server ${deps})
install( install(

1
robossembler_servers/package.xml
View file

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