diff --git a/rbs_mill_assist/CMakeLists.txt b/rbs_mill_assist/CMakeLists.txt index 43e6bc2..84deb0d 100644 --- a/rbs_mill_assist/CMakeLists.txt +++ b/rbs_mill_assist/CMakeLists.txt @@ -48,9 +48,13 @@ install( DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY launch urdf config world assets meshes + +install(DIRECTORY world urdf meshes launch config assets bt/xmls bt/config DESTINATION share/${PROJECT_NAME}) +add_subdirectory(scripts) +add_subdirectory(bt) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/rbs_mill_assist/assets/bunker/meshes/bunker.stl b/rbs_mill_assist/assets/bunker/meshes/bunker.stl new file mode 100644 index 0000000..d4ef85c Binary files /dev/null and b/rbs_mill_assist/assets/bunker/meshes/bunker.stl differ diff --git a/rbs_mill_assist/assets/bunker/model.config b/rbs_mill_assist/assets/bunker/model.config new file mode 100644 index 0000000..6c7bb6b --- /dev/null +++ b/rbs_mill_assist/assets/bunker/model.config @@ -0,0 +1,16 @@ + + + + bunker + 0.1 + model.sdf + + + Bill Finger + ur.narmak@gmail.com + + + + Empty + + diff --git a/rbs_mill_assist/assets/bunker/model.sdf b/rbs_mill_assist/assets/bunker/model.sdf new file mode 100644 index 0000000..ede8a4e --- /dev/null +++ b/rbs_mill_assist/assets/bunker/model.sdf @@ -0,0 +1,49 @@ + + + + + + + + + model://bunker/meshes/bunker.stl + + + + + + + + + + + + + + + + + + + + + + + + + model://bunker/meshes/bunker.stl + + + + + + + + + + + + + + + diff --git a/rbs_mill_assist/assets/laser/meshes/laser.stl b/rbs_mill_assist/assets/laser/meshes/laser.stl new file mode 100644 index 0000000..a15df0e Binary files /dev/null and b/rbs_mill_assist/assets/laser/meshes/laser.stl differ diff --git a/rbs_mill_assist/assets/laser/model.config b/rbs_mill_assist/assets/laser/model.config new file mode 100644 index 0000000..08762f2 --- /dev/null +++ b/rbs_mill_assist/assets/laser/model.config @@ -0,0 +1,16 @@ + + + + laser + 0.1 + model.sdf + + + Bill Finger + ur.narmak@gmail.com + + + + Empty + + diff --git a/rbs_mill_assist/assets/laser/model.sdf b/rbs_mill_assist/assets/laser/model.sdf new file mode 100644 index 0000000..0713e03 --- /dev/null +++ b/rbs_mill_assist/assets/laser/model.sdf @@ -0,0 +1,50 @@ + + + + + 0 0 0 1.57 0 0 + + + + + model://laser/meshes/laser.stl + + + + + + + + + + + + + + + + + + + + + + + + + model://laser/meshes/laser.stl + + + + + + + + + + + + + + + diff --git a/rbs_mill_assist/assets/shildik/model.sdf b/rbs_mill_assist/assets/shildik/model.sdf index 84c495a..1674d34 100644 --- a/rbs_mill_assist/assets/shildik/model.sdf +++ b/rbs_mill_assist/assets/shildik/model.sdf @@ -2,6 +2,7 @@ + diff --git a/rbs_mill_assist/bt/CMakeLists.txt b/rbs_mill_assist/bt/CMakeLists.txt new file mode 100644 index 0000000..70f2b16 --- /dev/null +++ b/rbs_mill_assist/bt/CMakeLists.txt @@ -0,0 +1,33 @@ + +find_package(behaviortree_ros2 REQUIRED) +find_package(behaviortree_cpp REQUIRED) +find_package(rbs_skill_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +# find_package(std_srvs REQUIRED) + +# Behaviortree interfaces +set(dependencies + rclcpp + rbs_skill_interfaces + geometry_msgs + behaviortree_ros2 + std_srvs +) + +add_library(vacuum_gripper_toggle SHARED plugins/vacuum_gripper_toggle.cpp) +list(APPEND plugin_libs vacuum_gripper_toggle) + +add_library(get_grasp_place_pose SHARED plugins/get_grasp_place_pose.cpp) +list(APPEND plugin_libs get_grasp_place_pose) + +foreach(bt_plugin ${plugin_libs}) + ament_target_dependencies(${bt_plugin} ${dependencies}) + target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) +endforeach() + +install(TARGETS + ${plugin_libs} + ARCHIVE DESTINATION share/${PROJECT_NAME}/bt_plugins + LIBRARY DESTINATION share/${PROJECT_NAME}/bt_plugins + RUNTIME DESTINATION share/${PROJECT_NAME}/bt_plugins +) diff --git a/rbs_mill_assist/bt/config/bt_executor.yaml b/rbs_mill_assist/bt/config/bt_executor.yaml new file mode 100644 index 0000000..6feace9 --- /dev/null +++ b/rbs_mill_assist/bt/config/bt_executor.yaml @@ -0,0 +1,13 @@ +bt_action_server: + ros__parameters: + action_name: "behavior_server" # Optional (defaults to `bt_action_server`) + tick_frequency: 100 # Optional (defaults to 100 Hz) + groot2_port: 1667 # Optional (defaults to 1667) + ros_plugins_timeout: 1000 # Optional (defaults 1000 ms) + + plugins: + - rbs_mill_assist/bt_plugins + - rbs_bt_executor/bt_plugins + + behavior_trees: + - rbs_mill_assist/xmls diff --git a/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp b/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp new file mode 100644 index 0000000..7eeb671 --- /dev/null +++ b/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp @@ -0,0 +1,118 @@ +#include "behaviortree_ros2/bt_service_node.hpp" + +#include "behaviortree_ros2/plugins.hpp" +#include +// #include +#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" +#include +#include +#include +#include +// #include +#include +#include + +using GraspingService = rbs_skill_interfaces::srv::GetPickPlacePoses; +using namespace BT; + +class Grasping : public RosServiceNode { +public: + Grasping(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosServiceNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting GetGraspPose"); + } + + static PortsList providedPorts() { + return providedBasicPorts( + {InputPort("object_name"), + InputPort("action_name"), + OutputPort>("pose"), + OutputPort>("pre_pose"), + OutputPort>("post_pose")}); + } + + bool setRequest(Request::SharedPtr &request) override { + RCLCPP_INFO(this->logger(), "Starting send request for: [%s]", + this->service_name_.c_str()); + if (!getInput("action_name", request->action_name)) { + RCLCPP_ERROR(this->node_.lock()->get_logger(), + "Failed to get object_name from input port"); + return false; + } + return true; + } + + NodeStatus onResponseReceived(const Response::SharedPtr &response) override { + if (!response->ok) { + RCLCPP_ERROR(this->node_.lock()->get_logger(), + "Response indicates failure."); + return NodeStatus::FAILURE; + } + + RCLCPP_INFO(this->node_.lock()->get_logger(), + "Response received successfully."); + + auto logPose = [this](const std::string &pose_name, + const geometry_msgs::msg::Pose &pose) { + RCLCPP_INFO(this->node_.lock()->get_logger(), + "%s:\n" + " Position: x = %.4f, y = %.4f, z = %.4f\n" + " Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f", + pose_name.c_str(), pose.position.x, pose.position.y, + pose.position.z, pose.orientation.x, pose.orientation.y, + pose.orientation.z, pose.orientation.w); + }; + if (!response->grasp.empty()) { + RCLCPP_INFO(this->logger(), "Got Pick Response"); + std::vector poses = {"Pregrasp Pose", "Grasp Pose", + "Postgrasp Pose"}; + + for (size_t n = 0; n < poses.size() && n < response->grasp.size(); ++n) { + logPose(poses[n], response->grasp.at(n)); + } + + auto grasp_pose = std::make_shared(); + auto pregrasp_pose = std::make_shared(); + auto postgrasp_pose = std::make_shared(); + + *pregrasp_pose = response->grasp.at(0); + *grasp_pose = response->grasp.at(1); + *postgrasp_pose = response->grasp.at(2); + + setOutput("pre_pose", pregrasp_pose); + setOutput("pose", grasp_pose); + setOutput("post_pose", postgrasp_pose); + return NodeStatus::SUCCESS; + } + if (!response->place.empty()) { + RCLCPP_INFO(this->logger(), "Got Place Response"); + std::vector poses = {"Preplace Pose", "Place Pose", + "Postplace Pose"}; + + for (size_t n = 0; n < poses.size() && n < response->place.size(); ++n) { + logPose(poses[n], response->place.at(n)); + } + + auto place_pose = std::make_shared(); + auto preplace_pose = std::make_shared(); + auto postplace_pose = std::make_shared(); + + *preplace_pose = response->place.at(0); + *place_pose = response->place.at(1); + *postplace_pose = response->place.at(2); + + setOutput("pre_pose", preplace_pose); + setOutput("pose", place_pose); + setOutput("post_pose", postplace_pose); + return NodeStatus::SUCCESS; + } + + RCLCPP_FATAL(this->logger(), "Could not response grasp pose"); + return NodeStatus::FAILURE; + } + // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {} +}; + +CreateRosNodePlugin(Grasping, "GetGraspPlacePose"); diff --git a/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp b/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp new file mode 100644 index 0000000..a7a2fb1 --- /dev/null +++ b/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp @@ -0,0 +1,47 @@ +#include "behaviortree_ros2/bt_service_node.hpp" + +#include "behaviortree_ros2/plugins.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include +#include +#include +#include + +using ToggleVacuumGripperService = std_srvs::srv::SetBool; +using namespace BT; + +class ToggleVacuumGrippper : public RosServiceNode { +public: + ToggleVacuumGrippper(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosServiceNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting ToggleVacuumGrippper"); + } + + static PortsList providedPorts() { + return providedBasicPorts({InputPort("enable")}); + } + + bool setRequest(Request::SharedPtr &request) override { + RCLCPP_INFO(this->logger(), "Starting send request"); + if (!getInput("enable", request->data)) { + RCLCPP_ERROR(this->node_.lock()->get_logger(), + "Failed to get sending data from input port"); + return false; + } + return true; + } + + NodeStatus onResponseReceived(const Response::SharedPtr &response) override { + if (!response->success) { + RCLCPP_ERROR(this->logger(), "Response indicates failure."); + return NodeStatus::FAILURE; + } + + return NodeStatus::SUCCESS; + } + // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {} +}; + +CreateRosNodePlugin(ToggleVacuumGrippper, "ToggleVacuumGrippper"); diff --git a/rbs_mill_assist/bt/xmls/Grasp.xml b/rbs_mill_assist/bt/xmls/Grasp.xml new file mode 100644 index 0000000..4d1bde0 --- /dev/null +++ b/rbs_mill_assist/bt/xmls/Grasp.xml @@ -0,0 +1,21 @@ + + + + + +