diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml b/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml index be441b6..81beeb7 100644 --- a/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml +++ b/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml @@ -3,6 +3,7 @@ + @@ -19,6 +20,9 @@ + + + diff --git a/robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp index 0709e7e..d236f20 100644 --- a/robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp +++ b/robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp @@ -24,9 +24,7 @@ public: static BT::PortsList providedPorts() { return providedBasicPorts({ - BT::InputPort("frame"), - BT::InputPort("printer"), - BT::InputPort("material") + BT::InputPort("frame") }); } diff --git a/robossembler/src/behavior_tree_nodes/Print.cpp b/robossembler/src/behavior_tree_nodes/Print.cpp index cad150a..947f54c 100644 --- a/robossembler/src/behavior_tree_nodes/Print.cpp +++ b/robossembler/src/behavior_tree_nodes/Print.cpp @@ -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("frame", frame_); - getInput("printer", printer_); - getInput("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(); request->name = frame_; - request->robot_namespace = ""; request->initial_pose = pose; request->xml = xml; RCLCPP_INFO(_node->get_logger(), "Connecting to '/spawn_entity' service..."); diff --git a/robossembler/src/move_controller_node.cpp b/robossembler/src/move_controller_node.cpp index 2b1a923..607214d 100644 --- a/robossembler/src/move_controller_node.cpp +++ b/robossembler/src/move_controller_node.cpp @@ -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))")); }