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

merge with print bt node

Browse source
This commit is contained in:
Ilya Uraev 2022-02-11 21:07:09 +04:00
parent c6df239ae1
commit 85de7fa683
4 changed files with 14 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -3,6 +3,7 @@
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Sequence>
<Action ID="Print" frame="${arg2}" server_name="/spawn_entity" server_timeout="10"/>
<Action ID="GetEntityState" part_name="${arg2}" 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="${arg1}" server_name="/move_to_joint_states" server_timeout="10" cancel_timeout="5" />
@ -19,6 +20,9 @@
<Action ID="MoveGripper">
<input_port name="arm_group"/>
</Action>
<Action ID="Print">
<input_port name="frame"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

4
robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp
View file

@ -24,9 +24,7 @@ public:
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>("frame"),
BT::InputPort<std::string>("printer"),
BT::InputPort<std::string>("material")
BT::InputPort<std::string>("frame")
});
}

14
robossembler/src/behavior_tree_nodes/Print.cpp
View file

@ -20,7 +20,7 @@ Print::Print(const std::string &xml_tag_name,
geometry_msgs::msg::Pose b;
a.position.x = -0.5;
a.position.y = -0.5;
a.position.z = 0.0;
a.position.z = 0.05;
printer_coords_.insert(std::make_pair("printerB", b));
printer_coords_.insert(std::make_pair("printerA", a));
}
@ -29,10 +29,12 @@ SpawnEntitySrv::Request::SharedPtr Print::populate_request()
{
std::string frame, printer, material;
getInput<std::string>("frame", frame_);
getInput<std::string>("printer", printer_);
getInput<std::string>("material", material_);
std::string package_share_directory = ament_index_cpp::get_package_share_directory("robossembler_sdf_models");
printer_ = "printerA";
material_="material1";
std::string package_share_directory = ament_index_cpp::get_package_share_directory("sdf_models");
std::filesystem::path sdf_path = package_share_directory + "/models/" + frame_ + "/model.sdf";
RCLCPP_INFO(_node->get_logger(), "Start read frame (%s) frame_path (%s)", frame_.c_str(), sdf_path.c_str());
@ -46,8 +48,7 @@ SpawnEntitySrv::Request::SharedPtr Print::populate_request()
} else {
RCLCPP_WARN(_node->get_logger(), "Failed while try to open file (%s)", sdf_path.c_str());
// TODO:
// - exit from service
return 0;
}
std::string xml = buffer.str();
@ -56,7 +57,6 @@ SpawnEntitySrv::Request::SharedPtr Print::populate_request()
auto request = std::make_shared<SpawnEntitySrv::Request>();
request->name = frame_;
request->robot_namespace = "";
request->initial_pose = pose;
request->xml = xml;
RCLCPP_INFO(_node->get_logger(), "Connecting to '/spawn_entity' service...");

4
robossembler/src/move_controller_node.cpp
View file

@ -55,10 +55,10 @@ public:
void init_knowledge()
{
problem_expert_->addInstance(plansys2::Instance{"rasmt_arm_group", "robot"});
problem_expert_->addInstance(plansys2::Instance{"rasmt", "part"});
problem_expert_->addInstance(plansys2::Instance{"cube", "part"});
problem_expert_->addInstance(plansys2::Instance("rasmt_hand_arm_group", "gripper"));
problem_expert_->setGoal(plansys2::Goal("(and(enviroment_setup rasmt_arm_group rasmt_hand_arm_group rasmt))"));
problem_expert_->setGoal(plansys2::Goal("(and(enviroment_setup rasmt_arm_group rasmt_hand_arm_group cube))"));
}