robossembler/runtime
7
0
Fork 0
Code Issues 5 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"> <BehaviorTree ID="BehaviorTree">
<Sequence> <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="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="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" /> <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"> <Action ID="MoveGripper">
<input_port name="arm_group"/> <input_port name="arm_group"/>
</Action> </Action>
<Action ID="Print">
<input_port name="frame"/>
</Action>
</TreeNodesModel> </TreeNodesModel>
<!-- ////////// --> <!-- ////////// -->
</root> </root>

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

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

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; geometry_msgs::msg::Pose b;
a.position.x = -0.5; a.position.x = -0.5;
a.position.y = -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("printerB", b));
printer_coords_.insert(std::make_pair("printerA", a)); printer_coords_.insert(std::make_pair("printerA", a));
} }
@ -29,10 +29,12 @@ SpawnEntitySrv::Request::SharedPtr Print::populate_request()
{ {
std::string frame, printer, material; std::string frame, printer, material;
getInput<std::string>("frame", frame_); 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"; 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()); 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 { } else {
RCLCPP_WARN(_node->get_logger(), "Failed while try to open file (%s)", sdf_path.c_str()); RCLCPP_WARN(_node->get_logger(), "Failed while try to open file (%s)", sdf_path.c_str());
// TODO: return 0;
// - exit from service
} }
std::string xml = buffer.str(); std::string xml = buffer.str();
@ -56,7 +57,6 @@ SpawnEntitySrv::Request::SharedPtr Print::populate_request()
auto request = std::make_shared<SpawnEntitySrv::Request>(); auto request = std::make_shared<SpawnEntitySrv::Request>();
request->name = frame_; request->name = frame_;
request->robot_namespace = "";
request->initial_pose = pose; request->initial_pose = pose;
request->xml = xml; request->xml = xml;
RCLCPP_INFO(_node->get_logger(), "Connecting to '/spawn_entity' service..."); 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() void init_knowledge()
{ {
problem_expert_->addInstance(plansys2::Instance{"rasmt_arm_group", "robot"}); 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_->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))"));
} }