diff --git a/env_components/assemble_component/CMakeLists.txt b/env_components/assemble_component/CMakeLists.txt deleted file mode 100644 index 81151de..0000000 --- a/env_components/assemble_component/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(assemble_component) - -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -set(CMAKE_CXX_STANDARD_REQUIRED True) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(rbs_skill_interfaces REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(env_manager REQUIRED) - -add_library(assemble_state_component SHARED - src/assemble_state_component.cpp) -target_compile_definitions(assemble_state_component - PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(assemble_state_component - "rbs_skill_interfaces" - "rclcpp" - "rclcpp_components" - "tf2_ros" - "tf2_msgs" - "geometry_msgs" - "env_manager") -rclcpp_components_register_nodes(assemble_state_component "assemble_component::AssembleStateComponent") -set(node_plugins "${node_plugins}assemble_component::AssembleStateComponent;$\n") - -install(TARGETS assemble_state_component - DESTINATION lib) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/env_components/assemble_component/src/assemble_state_component.cpp b/env_components/assemble_component/src/assemble_state_component.cpp deleted file mode 100644 index e00042b..0000000 --- a/env_components/assemble_component/src/assemble_state_component.cpp +++ /dev/null @@ -1,233 +0,0 @@ -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -namespace assemble_component -{ - -const std::string ASSEMBLE_PACKAGE_NAME_PARAM = "assemble_package_name"; -const std::string ASSEMBLE_PREFIX_NAME = "ASSEMBLE"; -const double ASSEMBLE_POSITION_OFFSET = 0.5; -const double ASSEMBLE_ORIENTATION_OFFSET = 0.5; - -/* TODO: implement sdf parse for assemble_solver. - * */ -struct assemble_solver -{ - std::string package_name; - std::string assemble_name; - std::string part_name; - std::string workspace; - std::map assemble_parts; -}; - - -class AssembleStateComponent: public env_manager::component_manager - ::ServiceComponent -{ -public: - AssembleStateComponent(const rclcpp::NodeOptions& options) - : env_manager::component_manager - ::ServiceComponent(options) - { - this->declare_parameter(ASSEMBLE_PACKAGE_NAME_PARAM, NULL); - param_subscriber_ = std::make_shared(this); - auto pevcallback = [this](const rclcpp::Parameter & p) { - if (ASSEMBLE_PACKAGE_NAME_PARAM == p.get_name() && - assemble_package_name_ != p.as_string() && - current_state_ == AssembleReqState::NONE) - assemble_package_name_ = p.as_string(); - else - RCLCPP_WARN(get_logger(), - "Unable to change param while assemble process in progress."); - }; - cb_handle_ = param_subscriber_->add_parameter_callback( - ASSEMBLE_PACKAGE_NAME_PARAM, pevcallback); - tf_buffer_ = - std::make_unique(get_clock()); - tf_listener_ = - std::make_shared(*tf_buffer_); - } - - void callback( - std::shared_ptr request, - std::shared_ptr response) - { - auto state = static_cast(request->req_kind); - auto call_status = false; - switch(state) - { - case AssembleReqState::NONE: - response->call_status = false; - return; - case AssembleReqState::INITIALIZE: - call_status = (current_state_ != AssembleReqState::NONE)? false: - on_initialize(request->assemble_name, request->part_name, request->workspace); - if (!call_status) - RCLCPP_WARN_STREAM(get_logger(), "callback: " << "initialize error."); - break; - case AssembleReqState::VALIDATE: - call_status = (current_state_ == AssembleReqState::INITIALIZE); - response->validate_status = (call_status)? on_validate(): call_status; - if (!call_status) - RCLCPP_WARN_STREAM(get_logger(), "callback: " << "validate error"); - break; - case AssembleReqState::COMPLETE: - call_status = (current_state_ != AssembleReqState::VALIDATE)? false: on_complete(); - if (!call_status) - RCLCPP_WARN_STREAM(get_logger(), "callback: " << "complete process error."); - break; - case AssembleReqState::CANCEL: - call_status = (current_state_ == AssembleReqState::NONE)? false: on_cancel(); - if (!call_status) - RCLCPP_WARN_STREAM(get_logger(), "callback: " << "complete process error."); - break; - } - response->call_status = call_status; - } - - bool on_initialize( - const std::string &assemble_name, - const std::string &part_name, - const std::string &workspace) - { - if (current_state_ != AssembleReqState::NONE) - { - RCLCPP_WARN_STREAM(get_logger(), "on_initialize: " << "assemble in progress"); - return false; - } - try { - mt.lock(); - assemble_ = (assemble_solver){ - .package_name=assemble_package_name_, - .assemble_name=assemble_name, - .part_name=part_name, - .workspace=workspace}; - mt.unlock(); - } catch (std::exception &ex) { - RCLCPP_ERROR_STREAM(get_logger(), "on_initialize: " << ex.what()); - return false; - } - current_state_ = AssembleReqState::INITIALIZE; - tf_broadcaster_ = - std::make_unique(this); - send_transform(assemble_); - - return true; - } - bool on_validate() - { - std::string frame_from = ASSEMBLE_PREFIX_NAME + assemble_.part_name; - std::string frame_to = assemble_.part_name; - - geometry_msgs::msg::TransformStamped t; - try { - t = tf_buffer_->lookupTransform( - frame_to, frame_from, tf2::TimePointZero); - } catch (const tf2::TransformException &ex) { - RCLCPP_WARN_STREAM(get_logger(), - "Could not transform " - << frame_to << " to " << frame_from - << " " << ex.what()); - return false; - } - - auto pos_validate = - t.transform.translation.x < ASSEMBLE_POSITION_OFFSET && - t.transform.translation.y < ASSEMBLE_POSITION_OFFSET && - t.transform.translation.z < ASSEMBLE_POSITION_OFFSET; - auto orient_validate = - t.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET && - t.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET && - t.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET && - t.transform.rotation.w < ASSEMBLE_ORIENTATION_OFFSET; - - return pos_validate && orient_validate; - } - bool on_complete() - { - try { - tf_broadcaster_.reset(); - } catch (const std::exception &ex) { - RCLCPP_ERROR_STREAM(get_logger(), "on_complete: " << ex.what()); - return false; - } - current_state_ = AssembleReqState::NONE; - assemble_ = {}; - return true; - } - bool on_cancel() - { - return on_complete(); - } - -protected: - enum AssembleReqState - { - NONE = -1, - INITIALIZE = 0, - VALIDATE = 1, - COMPLETE = 2, - CANCEL = 3 - }; - -private: - - void send_transform(const assemble_solver &solver) - { - if (solver.assemble_parts.empty()) - { - RCLCPP_WARN_STREAM( - get_logger(), "send_transform: " << "solver parts are empty"); - return; - } - for (const auto& pt: solver.assemble_parts) - { - geometry_msgs::msg::TransformStamped t; - t.header.frame_id = solver.workspace; - t.child_frame_id = pt.first.c_str(); - auto pose = pt.second.pose; - t.transform.translation.x = pose.position.x; - t.transform.translation.y = pose.position.y; - t.transform.translation.z = pose.position.z; - t.transform.rotation.x = pose.orientation.x; - t.transform.rotation.y = pose.orientation.y; - t.transform.rotation.z = pose.orientation.z; - t.transform.rotation.w = pose.orientation.w; - tf_broadcaster_->sendTransform(t); - } - } - - std::string assemble_package_name_; - AssembleReqState current_state_; - - std::shared_ptr param_subscriber_; - std::shared_ptr cb_handle_; - - std::shared_ptr tf_listener_{nullptr}; - std::unique_ptr tf_buffer_; - std::unique_ptr tf_broadcaster_; - assemble_solver assemble_; - - std::mutex mt; -}; -} // namespace assemble_component -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(assemble_component::AssembleStateComponent) diff --git a/env_components/component_interfaces/CMakeLists.txt b/env_components/component_interfaces/CMakeLists.txt new file mode 100644 index 0000000..4ad69bf --- /dev/null +++ b/env_components/component_interfaces/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(component_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/DetectObject.srv" + DEPENDENCIES std_msgs) + +ament_package() diff --git a/env_components/component_interfaces/package.xml b/env_components/component_interfaces/package.xml new file mode 100644 index 0000000..b9b61c0 --- /dev/null +++ b/env_components/component_interfaces/package.xml @@ -0,0 +1,20 @@ + + + + component_interfaces + 0.0.0 + TODO: Package description + splinter1984 + Apache-2.0 + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + std_msgs + + + ament_cmake + + diff --git a/env_components/component_interfaces/srv/DetectObject.srv b/env_components/component_interfaces/srv/DetectObject.srv new file mode 100644 index 0000000..cab64c2 --- /dev/null +++ b/env_components/component_interfaces/srv/DetectObject.srv @@ -0,0 +1,10 @@ +uint8 DETECT=0 +uint8 FOLLOW=1 +uint8 CANCEL=2 + +string object_name +uint8 req_kind +--- + +bool call_status +string error_msg diff --git a/env_components/scene_component/CMakeLists.txt b/env_components/scene_component/CMakeLists.txt new file mode 100644 index 0000000..e756164 --- /dev/null +++ b/env_components/scene_component/CMakeLists.txt @@ -0,0 +1,118 @@ +cmake_minimum_required(VERSION 3.5) +project(scene_component) + +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +set(CMAKE_CXX_STANDARD_REQUIRED True) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(image_transport REQUIRED) +find_package(ros_gz_bridge REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(env_manager REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(component_interfaces REQUIRED) + +find_package(ignition-transport11 REQUIRED) +set(GZ_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) + +find_package(ignition-msgs8 REQUIRED) +set(GZ_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) + +set(GZ_TARGET_PREFIX ignition) + +message(STATUS "Compiling against Ignition Fortress") + + +add_library(ign_tf2_broadcaster_component SHARED + src/ign_tf2_broadcaster_component.cpp) +target_compile_definitions(ign_tf2_broadcaster_component + PRIVATE "COMPOSITION_BUILDING_DLL") +target_link_libraries(ign_tf2_broadcaster_component + ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core + ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core + ) +ament_target_dependencies(ign_tf2_broadcaster_component + "ros_gz_bridge" + "rclcpp" + "rclcpp_components" + "env_manager" + "geometry_msgs" + "tf2_ros") +rclcpp_components_register_nodes(ign_tf2_broadcaster_component "scene_component::IgnTf2Broadcaster") +set(node_plugins "${node_plugins}scene_component::IgnTf2Broadcaster;$\n") + +add_library(ign_detect_object_component SHARED + src/ign_detect_object_component.cpp) +target_compile_definitions(ign_detect_object_component + PRIVATE "COMPOSITION_BUILDING_DLL") +target_link_libraries(ign_detect_object_component + ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core + ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core + ) +ament_target_dependencies(ign_detect_object_component + "ros_gz_bridge" + "rclcpp" + "rclcpp_components" + "env_manager" + "geometry_msgs" + "tf2_ros" + "component_interfaces") + rclcpp_components_register_nodes(ign_detect_object_component "scene_component::IgnDetectOpjectService") + set(node_plugins "${node_plugins}scene_component::IgnDetectObjectService;$\n") + +add_library(moveit_scene_component SHARED + src/moveit_scene_component.cpp) +target_compile_definitions(moveit_scene_component + PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(moveit_scene_component + "rclcpp" + "rclcpp_components" + "env_manager" + "geometry_msgs" + "tf2_ros" + "moveit_core" + "moveit_msgs" + "moveit_ros_planning" + "moveit_ros_planning_interface") +rclcpp_components_register_nodes(moveit_scene_component "scene_component::MoveitSceneComponent") +set(node_plugins "${node_plugins}scene_component::MoveitSceneComponent;$\n") + +install( + TARGETS + ign_detect_object_component + ign_tf2_broadcaster_component + moveit_scene_component + DESTINATION + lib) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/env_components/assemble_component/package.xml b/env_components/scene_component/package.xml similarity index 60% rename from env_components/assemble_component/package.xml rename to env_components/scene_component/package.xml index ee1ea3a..a0a6846 100644 --- a/env_components/assemble_component/package.xml +++ b/env_components/scene_component/package.xml @@ -1,7 +1,7 @@ - assemble_component + scene_component 0.0.0 TODO: Package description splinter1984 @@ -10,13 +10,17 @@ ament_cmake pkg-config - rbs_skill_interfaces + image_transport + ros_gz_bridge rclcpp rclcpp_components geometry_msgs - tf2_ros env_manager - tf2_msgs + + ignition-msgs8 + ignition-transport11 + ignition-msgs8 + ignition-transport11 ament_lint_auto ament_lint_common diff --git a/env_components/scene_component/src/ign_detect_object_component.cpp b/env_components/scene_component/src/ign_detect_object_component.cpp new file mode 100644 index 0000000..bf908d1 --- /dev/null +++ b/env_components/scene_component/src/ign_detect_object_component.cpp @@ -0,0 +1,109 @@ +#include "component_manager/service_component.hpp" +#include "component_interfaces/srv/detect_object.hpp" + +#include +#include +#include +#include + +#include +#include + +#define IGN_DEFAULT_BASE_FRAME "world" +#define IGN_DEFAULT_WORLD_NAME "mir" + +namespace scene_component +{ + +class IgnDetectObjectService: public + env_manager::component_manager::ServiceComponent +{ + public: + IgnDetectObjectService(const rclcpp::NodeOptions &options) + : env_manager::component_manager::ServiceComponent(options) + { + std::string ign_topic_name = "/world/" + std::string(IGN_DEFAULT_WORLD_NAME) + "/dynamic_pose/info"; + _tf2_broadcaster = std::make_unique(this); + _ign_node = std::make_shared(); + _ign_node->Subscribe(ign_topic_name, &IgnDetectObjectService::on_pose, this); + } + + void callback( + std::shared_ptr request, + std::shared_ptr response) + { + auto kind = static_cast(request->req_kind); + bool status = false; + switch(kind) + { + case DETECT: { + (void) kind; + break; + } case FOLLOW: { + auto ok = std::find(_follow_frames.begin(), _follow_frames.end(), + request->object_name) == _follow_frames.end(); + if (ok) + _follow_frames.push_back(request->object_name); + status = ok; + break; + } case CANCEL: { + auto it = std::find(_follow_frames.begin(), _follow_frames.end(), + request->object_name); + auto ok = it != _follow_frames.end(); + if (ok) + _follow_frames.erase(it); + status = ok; + break; + } + } + response->call_status = status; + } + + private: + enum DetectObjectReqKind + { + DETECT=0, + FOLLOW=1, + CANCEL=2 + }; + + void on_pose(const ignition::msgs::Pose_V &pose_v) + { + auto poses = pose_v.pose(); + for (const auto& obj: _follow_frames) + { + auto it = std::find_if(poses.begin(), poses.end(), + [&obj](const auto& pose){ return pose.name() == obj;}); + if (it == poses.end()) + continue; + + geometry_msgs::msg::PoseStamped rpose; + ros_gz_bridge::convert_gz_to_ros(*it, rpose); + geometry_msgs::msg::TransformStamped t; + + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = IGN_DEFAULT_BASE_FRAME; + t.child_frame_id = it->name(); + t.transform.translation.x = rpose.pose.position.x; + t.transform.translation.y = rpose.pose.position.y; + t.transform.translation.z = rpose.pose.position.z; + t.transform.rotation.x = rpose.pose.orientation.x; + t.transform.rotation.y = rpose.pose.orientation.y; + t.transform.rotation.z = rpose.pose.orientation.z; + t.transform.rotation.w = rpose.pose.orientation.w; + + _tf2_broadcaster->sendTransform(t); + } + } + + std::vector _follow_frames; + std::unique_ptr _tf2_broadcaster; + std::shared_ptr _ign_node; + +}; + +} // namespace scene_component + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnDetectObjectService) diff --git a/env_components/scene_component/src/ign_tf2_broadcaster_component.cpp b/env_components/scene_component/src/ign_tf2_broadcaster_component.cpp new file mode 100644 index 0000000..bdd930f --- /dev/null +++ b/env_components/scene_component/src/ign_tf2_broadcaster_component.cpp @@ -0,0 +1,72 @@ +#include "component_manager/node_component.hpp" + +#include +#include +#include +#include + +#include +#include + +namespace scene_component +{ + +const std::string DEFAULT_NODE_NAME = "ign_tf2_broadcaster"; +const std::string DEFAULT_IGN_WORLD_NAME = "mir"; +const std::string DEFAULT_BASE_FRAME_NAME = "world"; + +class IgnTf2Broadcaster: public env_manager::component_manager::NodeComponent +{ +public: + IgnTf2Broadcaster(const rclcpp::NodeOptions & options) + : env_manager::component_manager::NodeComponent(options) + { + _follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6"}; + std::string topic_name = "/world/" + DEFAULT_IGN_WORLD_NAME + "/dynamic_pose/info"; + // init broadcaster + _tf2_broadcaster = std::make_unique(*this); + _ign_node = std::make_shared(); + _ign_node->Subscribe(topic_name, &IgnTf2Broadcaster::on_pose, this); + } + +private: + void on_pose(const ignition::msgs::Pose_V& pose_v) + { + for (const auto &it: pose_v.pose()) + { + if (std::find(_follow_frames.begin(), _follow_frames.end(), it.name()) == _follow_frames.end()) + continue; + + geometry_msgs::msg::PoseStamped rpose; + ros_gz_bridge::convert_gz_to_ros(it, rpose); + + geometry_msgs::msg::TransformStamped t; + + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = DEFAULT_BASE_FRAME_NAME; + t.child_frame_id = it.name(); + + t.transform.translation.x = rpose.pose.position.x; + t.transform.translation.y = rpose.pose.position.y; + t.transform.translation.z = rpose.pose.position.z; + + t.transform.rotation.x = rpose.pose.orientation.x; + t.transform.rotation.y = rpose.pose.orientation.y; + t.transform.rotation.z = rpose.pose.orientation.z; + t.transform.rotation.w = rpose.pose.orientation.w; + + RCLCPP_ERROR(get_logger(), "Provide transform for frame: %s", it.name().c_str()); + + _tf2_broadcaster->sendTransform(t); + } + } + + std::unique_ptr _tf2_broadcaster; + std::shared_ptr _ign_node; + std::vector _follow_frames; + +}; +} // namespace scene_component +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnTf2Broadcaster) diff --git a/env_components/scene_component/src/moveit_scene_component.cpp b/env_components/scene_component/src/moveit_scene_component.cpp new file mode 100644 index 0000000..44c3d1f --- /dev/null +++ b/env_components/scene_component/src/moveit_scene_component.cpp @@ -0,0 +1,115 @@ +#include + +#include + +#include +#include + +#include + +#include +#include +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "tf2/exceptions.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + +using namespace std::chrono_literals; + +namespace scene_component +{ + +const std::string DEFAULT_PLANNING_GOUP_NAME = "panda_arm"; +const std::string DEFAULT_SUBNODE_NS = "moveit_cpp_node"; +const std::string DEFAULT_BASE_FRAME = "world"; + +class MoveitSceneComponent: public env_manager::component_manager::NodeComponent +{ +public: + MoveitSceneComponent(const rclcpp::NodeOptions& options) + : env_manager::component_manager::NodeComponent(options) + { + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + + node_ = rclcpp::Node::make_shared(DEFAULT_SUBNODE_NS, "", node_options); + + moveit_cpp_ptr_ = std::make_shared(node_); + moveit_cpp_ptr_->getPlanningSceneMonitor()->providePlanningSceneService(); + planning_components_ = std::make_shared( + DEFAULT_PLANNING_GOUP_NAME, moveit_cpp_ptr_); + robot_model_ptr_ = moveit_cpp_ptr_->getRobotModel(); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + timer_ = this->create_wall_timer(1s, std::bind(&MoveitSceneComponent::on_timer, this)); + } + +private: + void on_timer() + { + auto available_frames = tf_buffer_->getAllFrameNames(); + auto robot_link_names = robot_model_ptr_->getLinkModelNames(); + + for (const auto &frame: available_frames) + { + if (std::find(robot_link_names.begin(), robot_link_names.end(), frame) != robot_link_names.end()) + continue; + + geometry_msgs::msg::TransformStamped t; + try { + t = tf_buffer_->lookupTransform( + DEFAULT_BASE_FRAME, frame, + tf2::TimePointZero); + } catch (const tf2::TransformException &ex) + { + RCLCPP_WARN(get_logger(), "Could not transform %s to %s: %s", + frame.c_str(), DEFAULT_BASE_FRAME.c_str(), ex.what()); + } + + moveit_msgs::msg::CollisionObject co; + co.header.frame_id = robot_model_ptr_->getRootLink()->getName(); + co.id = frame; + + //TODO: replace with stl model + shape_msgs::msg::SolidPrimitive obj; + obj.type = obj.BOX; + obj.dimensions = {0.1, 0.1, 0.1}; + + geometry_msgs::msg::Pose obj_pose; + obj_pose.position.x = t.transform.translation.x; + obj_pose.position.y = t.transform.translation.y; + obj_pose.position.z = t.transform.translation.z; + + obj_pose.orientation.x = t.transform.rotation.x; + obj_pose.orientation.y = t.transform.rotation.y; + obj_pose.orientation.z = t.transform.rotation.z; + obj_pose.orientation.w = t.transform.rotation.w; + + co.primitives.push_back(obj); + co.primitive_poses.push_back(obj_pose); + co.operation = co.ADD; + + { + planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr_->getPlanningSceneMonitor()); + scene->processCollisionObjectMsg(co); + } + } + } + + rclcpp::Node::SharedPtr node_; + std::shared_ptr moveit_cpp_ptr_; + moveit::core::RobotModelConstPtr robot_model_ptr_; + std::shared_ptr planning_components_; + std::shared_ptr tf_listener_{nullptr}; + std::unique_ptr tf_buffer_; + rclcpp::TimerBase::SharedPtr timer_{nullptr}; + + +}; +} // namesapce scene_component + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::MoveitSceneComponent) diff --git a/env_manager/config/environments/ground_true.lua b/env_manager/config/environments/ground_true.lua index 38c7f06..86da26e 100644 --- a/env_manager/config/environments/ground_true.lua +++ b/env_manager/config/environments/ground_true.lua @@ -2,18 +2,13 @@ GROUND_TRUE = { namespace = "ground_true", components = { - --[[ - talker_node = { - lib = "libpub_component.so", - class = "pub_component::Publisher", - }, - service_node = { - lib = "libsrv_component.so", - class = "srv_component::Service" - },]]-- camera_node = { lib = "libign_camera_component.so", class = "camera_component::IgnCameraComponent" }, + scene_component = { + lib = "libign_detect_object_component.so", + class = "scene_component::IgnDetectObjectService" + }, } } diff --git a/env_manager/config/nodes.lua b/env_manager/config/nodes.lua index 128dc53..9ab33b8 100644 --- a/env_manager/config/nodes.lua +++ b/env_manager/config/nodes.lua @@ -1,18 +1,12 @@ NODES = { - --[[ - talker_node = { - name = "talker", - type = "Publisher", - msg_type = "std_msgs/String", - }, - service_node = { - name = "add_two_ints", - type = "Service", - msg_type = "example_interfaces/AddTwoInts" - },]]-- camera_node = { name = "camera_node", type = "Publisher", msg_type = "sensor_msgs/Image" }, + scene_component = { + name = "scene_component", + type = "Service", + msg_type = "component_interfaces/DetectObject" + }, } diff --git a/env_manager/include/component_manager/node_component.hpp b/env_manager/include/component_manager/node_component.hpp new file mode 100644 index 0000000..482b698 --- /dev/null +++ b/env_manager/include/component_manager/node_component.hpp @@ -0,0 +1,36 @@ +#ifndef ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__ +#define ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__ + +#include "component_manager/visibility_control.h" + +#include "rclcpp/node.hpp" + +namespace env_manager +{ +namespace component_manager +{ + +const std::string DEFAULT_NODE_NODE_NAME = "env_manager_node"; + +//TODO: interhit all another components from this node_component +class NodeComponent: public rclcpp::Node +{ +public: + explicit NodeComponent(const rclcpp::NodeOptions& options) + :Node(DEFAULT_NODE_NODE_NAME, options) + { + auto ret = rcutils_logging_set_logger_level( + get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL); + if (ret != RCUTILS_RET_OK) + { + RCLCPP_ERROR(get_logger(), + "Error setting severity: %s", rcutils_get_error_string().str); + rcutils_reset_error(); + } + } +}; + +} // namesapce component_manager +} // namesapce env_manager + +#endif // ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__ diff --git a/env_manager/include/config/options.hpp b/env_manager/include/config/options.hpp index a1460e2..190c23f 100644 --- a/env_manager/include/config/options.hpp +++ b/env_manager/include/config/options.hpp @@ -33,7 +33,8 @@ struct NodeOption Publisher, Subscriber, Service, - Client + Client, + Node }; static NodeOption create_option( diff --git a/env_manager/package.xml b/env_manager/package.xml index b3fedd1..4a1128c 100644 --- a/env_manager/package.xml +++ b/env_manager/package.xml @@ -3,9 +3,9 @@ env_manager 0.0.0 - TODO: Package description + env_manager splinter1984 - TODO: License declaration + Apache-2.0 ament_cmake_ros diff --git a/env_manager/src/component_manager/component_manager.cpp b/env_manager/src/component_manager/component_manager.cpp index c8983c9..96bf858 100644 --- a/env_manager/src/component_manager/component_manager.cpp +++ b/env_manager/src/component_manager/component_manager.cpp @@ -1,4 +1,5 @@ #include "component_manager/component_manager.hpp" +#include "component_manager/node_component.hpp" #include "component_manager/publisher_component.hpp" #include "component_manager/subscriber_component.hpp" #include "component_manager/service_component.hpp" @@ -50,6 +51,9 @@ static rclcpp::NodeOptions create_options( case config::NodeOption::NodeType::Client: args.push_back(DEFAULT_CLIENT_NAME + ":=" + node_opts.name); break; + case config::NodeOption::NodeType::Node: + args.erase(args.end()); + break; } opts.arguments(args); @@ -87,8 +91,11 @@ void ComponentManager::register_components( auto loader = new class_loader::ClassLoader(comp.library); auto classes = loader->getAvailableClasses(); + for (auto clazz: classes) { + if (clazz != class_name) + continue; rclcpp::NodeOptions opts = create_default_options(ns); if (clazz == class_name) { diff --git a/env_manager/src/config/options.cpp b/env_manager/src/config/options.cpp index d6e2d94..2fbe53d 100644 --- a/env_manager/src/config/options.cpp +++ b/env_manager/src/config/options.cpp @@ -50,6 +50,7 @@ NodeOption::node_type_remap = {"Subscriber", NodeOption::NodeType::Subscriber}, {"Service", NodeOption::NodeType::Service}, {"Client", NodeOption::NodeType::Client}, + {"Node", NodeOption::NodeType::Node}, }; EnvManagerOption EnvManagerOption::create_option( diff --git a/rbs_bt_executor/CMakeLists.txt b/rbs_bt_executor/CMakeLists.txt index c333769..165ac0c 100644 --- a/rbs_bt_executor/CMakeLists.txt +++ b/rbs_bt_executor/CMakeLists.txt @@ -5,7 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() - find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) @@ -19,6 +18,7 @@ find_package(ament_index_cpp REQUIRED) find_package(rbs_skill_interfaces REQUIRED) find_package(behavior_tree REQUIRED) find_package(control_msgs REQUIRED) +find_package(component_interfaces REQUIRED) if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -38,6 +38,7 @@ set(dependencies behavior_tree std_msgs control_msgs + component_interfaces ) include_directories(include) @@ -57,13 +58,17 @@ list(APPEND plugin_libs rbs_skill_move_joint_state) add_library(rbs_add_planning_scene_object SHARED src/AddPlanningSceneObject.cpp) list(APPEND plugin_libs rbs_add_planning_scene_object) +add_library(rbs_assemble_process_state SHARED src/AssembleProcessState.cpp) +list(APPEND plugin_libs rbs_assemble_process_state) + +add_library(rbs_detect_object SHARED src/DetectObject.cpp) +list(APPEND plugin_libs rbs_detect_object) + foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${dependencies}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) endforeach() - - install(DIRECTORY launch bt_trees config DESTINATION share/${PROJECT_NAME}) install(TARGETS diff --git a/rbs_bt_executor/bt_trees/assemble.xml b/rbs_bt_executor/bt_trees/assemble.xml new file mode 100644 index 0000000..f3c7026 --- /dev/null +++ b/rbs_bt_executor/bt_trees/assemble.xml @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + ` + + + + + + + + + diff --git a/rbs_bt_executor/package.xml b/rbs_bt_executor/package.xml index fce45a2..e96c0d1 100644 --- a/rbs_bt_executor/package.xml +++ b/rbs_bt_executor/package.xml @@ -10,6 +10,7 @@ ament_cmake rbs_skill_interfaces + component_interfaces behavior_tree ament_lint_auto ament_lint_common diff --git a/rbs_bt_executor/src/AssembleProcessState.cpp b/rbs_bt_executor/src/AssembleProcessState.cpp new file mode 100644 index 0000000..a28419e --- /dev/null +++ b/rbs_bt_executor/src/AssembleProcessState.cpp @@ -0,0 +1,67 @@ +#include +#include +#include + +#include "rbs_skill_interfaces/srv/assemble_state.hpp" + +using AssembleStateSrv = rbs_skill_interfaces::srv::AssembleState; + +class AssembleState : public BtService +{ +public: + AssembleState(const std::string & name, const BT::NodeConfiguration &config) + : BtService(name, config) + { + RCLCPP_INFO_STREAM(_node->get_logger(), "Start node."); + } + + AssembleStateSrv::Request::SharedPtr populate_request() + { + auto request = std::make_shared(); + auto state_kind = getInput("StateKind").value(); + request->req_kind = -1; + + if (state_kind == "INITIALIZE") + request->req_kind = 0; + else if (state_kind == "VALIDATE") + request->req_kind = 1; + else if (state_kind == "COMPLETE") + request->req_kind = 2; + + auto assemble_name = getInput("AssembleName").value(); + auto part_name = getInput("PartName").value(); + auto workspace = getInput("WorkspaceName").value(); + + request->assemble_name = assemble_name; + request->part_name = part_name; + request->workspace = workspace; + + return request; + } + + BT::NodeStatus handle_response(AssembleStateSrv::Response::SharedPtr response) + { + // TODO: return bad status on validate process return false state + return (response->call_status = true)? BT::NodeStatus::SUCCESS: BT::NodeStatus::FAILURE; + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("StateKind"), + BT::InputPort("AssembleName"), + BT::InputPort("PartName"), + BT::InputPort("WorkspaceName") + }); + } + +private: + std::map assemble_states_; +}; + +#include "behaviortree_cpp_v3/bt_factory.h" + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("AssembleState"); +} diff --git a/rbs_bt_executor/src/DetectObject.cpp b/rbs_bt_executor/src/DetectObject.cpp new file mode 100644 index 0000000..037c0a9 --- /dev/null +++ b/rbs_bt_executor/src/DetectObject.cpp @@ -0,0 +1,51 @@ +#include +#include +#include + +#include + +using DetectObjectSrv = component_interfaces::srv::DetectObject; +class DetectObject: public BtService +{ +public: + DetectObject(const std::string &name, const BT::NodeConfiguration &config) + : BtService(name, config) + { + RCLCPP_INFO_STREAM(_node->get_logger(), "Start node."); + } + + DetectObjectSrv::Request::SharedPtr populate_request() + { + auto request = std::make_shared(); + auto req_kind = getInput("ReqKind").value(); + if (req_kind == "DETECT") + request->req_kind = 0; + else if (req_kind == "FOLLOW") + request->req_kind = 1; + else + request->req_kind = 2; + request->object_name = getInput("ObjectName").value(); + return request; + } + + BT::NodeStatus handle_response(DetectObjectSrv::Response::SharedPtr response) + { + return response->call_status? BT::NodeStatus::SUCCESS: BT::NodeStatus::FAILURE; + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("ReqKind"), + BT::InputPort("ObjectName"), + }); + } +}; + +#include "behaviortree_cpp_v3/bt_factory.h" + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("DetectObject"); +} + diff --git a/rbs_simulation/launch/rbs_simulation.launch.py b/rbs_simulation/launch/rbs_simulation.launch.py index ecb3fb4..bbc6341 100644 --- a/rbs_simulation/launch/rbs_simulation.launch.py +++ b/rbs_simulation/launch/rbs_simulation.launch.py @@ -260,7 +260,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( [os.path.join(get_package_share_directory('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')]), - launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])]) + launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])]) # Spawn robot gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create', arguments=[ @@ -380,16 +380,16 @@ def generate_launch_description(): ] ) - move_topose_action_server = Node( - package="rbs_skill_servers", - executable="move_topose_action_server", - parameters=[ - robot_description, - robot_description_semantic, - robot_description_kinematics, - {"use_sim_time": use_sim_time}, - ] - ) + #move_topose_action_server = Node( + # package="rbs_skill_servers", + # executable="move_topose_action_server", + # parameters=[ + # robot_description, + # robot_description_semantic, + # robot_description_kinematics, + # {"use_sim_time": use_sim_time}, + # ] + #) move_cartesian_path_action_server = Node( package="rbs_skill_servers", @@ -424,6 +424,16 @@ def generate_launch_description(): points_params ] ) + + assemble_state = Node( + package="rbs_skill_servers", + executable="assemble_state_service_server", + output="screen", + parameters=[ + {'assemble_prefix': 'ASSEMBLE_'}, + {'assemble_dir': '/home/splinter1984/tmp_ws/src/robossembler-ros2/rbs_task_planner/example/sdf_models/'} + ] + ) # add_planning_scene_object = Node( # package="rbs_skill_servers", @@ -452,7 +462,8 @@ def generate_launch_description(): move_cartesian_path_action_server, move_joint_state_action_server, grasp_pose_loader, + assemble_state, #add_planning_scene_object ] - return LaunchDescription(declared_arguments + nodes_to_start) \ No newline at end of file + return LaunchDescription(declared_arguments + nodes_to_start) diff --git a/rbs_simulation/worlds/mir.sdf b/rbs_simulation/worlds/mir.sdf index d9c8b98..3a24e5a 100644 --- a/rbs_simulation/worlds/mir.sdf +++ b/rbs_simulation/worlds/mir.sdf @@ -290,4 +290,4 @@ - \ No newline at end of file + diff --git a/rbs_skill_interfaces/srv/AssembleState.srv b/rbs_skill_interfaces/srv/AssembleState.srv index db982b1..50f3ce0 100644 --- a/rbs_skill_interfaces/srv/AssembleState.srv +++ b/rbs_skill_interfaces/srv/AssembleState.srv @@ -9,9 +9,6 @@ uint8 VALIDATE=1 # COMPLETE - finalize assemble process uint8 COMPLETE=2 -# CANCEL - force finalize assemble process -uint8 CANCEL=3 - string assemble_name string part_name uint8 req_kind @@ -23,6 +20,9 @@ string workspace bool call_status +# wich assemble is asociate with response status +string assemble_name + # on error string error_msg diff --git a/rbs_skill_servers/CMakeLists.txt b/rbs_skill_servers/CMakeLists.txt index 9474b62..3f442ae 100644 --- a/rbs_skill_servers/CMakeLists.txt +++ b/rbs_skill_servers/CMakeLists.txt @@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -30,6 +30,16 @@ find_package(rbs_skill_interfaces REQUIRED) find_package(rmw REQUIRED) find_package(tf2_eigen REQUIRED) find_package(tf2_msgs REQUIRED) +find_package(tinyxml2_vendor REQUIRED) +find_package(TinyXML2 REQUIRED) + +include_directories(SYSTEM + ${TINYXML2_INCLUDE_DIR} +) + +link_directories( + ${TINYXML2_LIBRARY_DIRS} +) set(deps rclcpp @@ -44,6 +54,7 @@ set(deps rbs_skill_interfaces tf2_eigen tf2_msgs + tinyxml2_vendor ) if(BUILD_TESTING) @@ -74,9 +85,6 @@ ament_target_dependencies(pick_place_pose_loader ${deps}) rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server) rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server) - - - add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp) ament_target_dependencies(move_to_joint_states_action_server ${deps}) @@ -92,6 +100,12 @@ ament_target_dependencies(move_cartesian_path_action_server ${deps}) add_executable(add_planning_scene_object_service src/add_planning_scene_objects_service.cpp) ament_target_dependencies(add_planning_scene_object_service ${deps}) +add_library(assemble_state_server SHARED src/assemble_state_server.cpp) +target_compile_definitions(assemble_state_server + PRIVATE "ASSEMBLE_STATE_SERVER_CPP_BUILDING_DLL") +ament_target_dependencies(assemble_state_server ${deps}) +target_link_libraries(assemble_state_server ${TINYXML2_LIBRARY}) +rclcpp_components_register_node(assemble_state_server PLUGIN "AssembleStateServer" EXECUTABLE assemble_state_service_server) install( DIRECTORY include/ @@ -106,6 +120,7 @@ install( move_to_joint_states_action_server move_cartesian_path_action_server add_planning_scene_object_service + assemble_state_server ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} diff --git a/rbs_skill_servers/src/assemble_state_server.cpp b/rbs_skill_servers/src/assemble_state_server.cpp new file mode 100644 index 0000000..d359528 --- /dev/null +++ b/rbs_skill_servers/src/assemble_state_server.cpp @@ -0,0 +1,245 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#define ASSEMBLE_PREFIX_PARAM_NAME "assemble_prefix" +#define SERVICE_NAME "assemble_state" + +#define ASSEMBLE_DIR_PARAM_NAME "assemble_dir" +#define ASSEMBLE_POSITION_OFFSET 0.5 +#define ASSEMBLE_ORIENTATION_OFFSET 0.5 + +#define ASSEMBLE_SDF_PATH(package_dir, assemble_name) \ + (package_dir) + "/models/" + (assemble_name) + "/model.sdf" + +static inline geometry_msgs::msg::Pose get_pose( + const std::vector & tokens) +{ + geometry_msgs::msg::Pose p; + p.position.set__x(std::stod(tokens.at(0))); + p.position.set__y(std::stod(tokens.at(1))); + p.position.set__z(std::stod(tokens.at(2))); + p.orientation.set__x(std::stod(tokens.at(3))); + p.orientation.set__y(std::stod(tokens.at(4))); + p.orientation.set__z(std::stod(tokens.at(5))); + + return p; +} + +static inline geometry_msgs::msg::PoseStamped get_pose_stamped( + const std::string & pose_string) +{ + std::stringstream ss(pose_string); + std::istream_iterator begin(ss); + std::istream_iterator end; + std::vector tokens(begin, end); + + geometry_msgs::msg::PoseStamped ps; + ps.set__pose(get_pose(tokens)); + return ps; +} + +static std::vector get_assemble_poses( + const std::string & assemble_name, const std::string & part_name, + const std::string& package_dir, const std::string & assemble_prefix) +{ + std::vector result; + std::filesystem::path sdf_path = package_dir + assemble_name + ".sdf"; + + tinyxml2::XMLDocument doc; + doc.LoadFile(sdf_path.c_str()); + auto model = doc.RootElement()->FirstChildElement("model"); + auto joint = model->FirstChildElement("joint"); + + while (joint) + { + auto frame_id = joint->FirstChildElement("child")->GetText(); + if (frame_id != part_name) + continue; + auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText()); + ps.header.set__frame_id(assemble_prefix + part_name); + result.push_back(ps); + joint = joint->NextSiblingElement("joint"); + } + + return result; +} + +class AssembleStateServer: public rclcpp::Node +{ +public: + AssembleStateServer(rclcpp::NodeOptions options) + : Node(SERVICE_NAME, options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) + { + assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string(); + assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string(); + tf_buffer_ = std::make_unique(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + service_ = create_service( + SERVICE_NAME, std::bind(&AssembleStateServer::callback, this, + std::placeholders::_1, std::placeholders::_2)); + } + + void callback( + std::shared_ptr request, + std::shared_ptr response) + { + auto state = static_cast(request->req_kind); + bool call_status = false; + std::string assemble_name = request->assemble_name; + std::string part_name = request->part_name; + auto assemble = assembles_.find(request->assemble_name); + if (assemble == assembles_.end()) + assembles_.insert(std::make_pair(request->assemble_name, Assemble { + .part=request->part_name, + .ws=request->workspace, + .state=NONE, + .poses=get_assemble_poses(assemble_name, part_name, assemble_dir_, assemble_prefix_) + })); + RCLCPP_INFO(get_logger(), "callback call with assemble name: %s", assemble_name.c_str()); + switch(state) + { + case NONE: + call_status = false; + break; + case INITIALIZE: + call_status = (assembles_.at(assemble_name).state == NONE) && + on_initialize(&assembles_.at(assemble_name)); + break; + case VALIDATE: + response->validate_status = (call_status = (assembles_.at(assemble_name).state == INITIALIZE)) && + on_validate(&assembles_.at(assemble_name)); + break; + case COMPLETE: + call_status = (assembles_.at(assemble_name).state == VALIDATE) && + on_complete(&assembles_.at(assemble_name)); + if (call_status) + assembles_.erase(assemble_name); + break; + } + + response->call_status = call_status; + response->assemble_name = assemble_name; + } + +private: + enum AssembleReqState + { + NONE=-1, + INITIALIZE=0, + VALIDATE=1, + COMPLETE=2 + }; + + struct Assemble + { + std::string part; + std::string ws; + AssembleReqState state; + std::vector poses; + std::unique_ptr tf2_broadcaster; + }; + + bool on_initialize(Assemble* assemble) + { + RCLCPP_INFO(get_logger(), "initialize assemble state for part: %s", assemble->part.c_str()); + try + { + assemble->tf2_broadcaster = std::make_unique(this); + } catch (const tf2::TransformException &ex){ + RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s", ex.what()); + return false; + } + assemble->state = INITIALIZE; + for (const auto& it: assemble->poses) + { + geometry_msgs::msg::TransformStamped t; + t.header.frame_id = assemble->ws; + t.child_frame_id = it.header.frame_id; + auto pose = it.pose; + t.transform.translation.x = pose.position.x; + t.transform.translation.y = pose.position.y; + t.transform.translation.z = pose.position.z; + t.transform.rotation.x = pose.orientation.x; + t.transform.rotation.y = pose.orientation.y; + t.transform.rotation.z = pose.orientation.z; + t.transform.rotation.w = pose.orientation.w; + assemble->tf2_broadcaster->sendTransform(t); + } + + return true; + } + + bool on_validate(Assemble* assemble) + { + RCLCPP_INFO(get_logger(), "validate assemble state for part: %s", assemble->part.c_str()); + std::string frame_from = assemble_prefix_ + assemble->part; + std::string frame_to = assemble->part; + geometry_msgs::msg::TransformStamped ts; + try + { + ts = tf_buffer_->lookupTransform(frame_to, frame_from, tf2::TimePointZero); + } catch (const tf2::TransformException &ex) { + return false; + } + auto pos_validate = ts.transform.translation.x < ASSEMBLE_POSITION_OFFSET && + ts.transform.translation.y < ASSEMBLE_POSITION_OFFSET && + ts.transform.translation.z < ASSEMBLE_POSITION_OFFSET; + auto orient_validate = ts.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET && + ts.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET && + ts.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET; + + RCLCPP_INFO(get_logger(), "pos_validate: %d, orient_validate: %d", pos_validate, orient_validate); + assemble->state = (pos_validate && orient_validate)? VALIDATE: INITIALIZE; + + return assemble->state == VALIDATE; + } + + bool on_complete(Assemble* assemble) + { + RCLCPP_INFO(get_logger(), "complete assemble state for part: %s", assemble->part.c_str()); + try + { + assemble->tf2_broadcaster.reset(); + assemble->tf2_broadcaster = NULL; + assemble->poses.clear(); + } catch (const std::exception &ex) { + RCLCPP_WARN(get_logger(), "something happen on tf2.reset(): %s", ex.what()); + return false; + } + + assemble->state = COMPLETE; + + return true; + } + + +private: + std::map assembles_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_buffer_; + std::unique_ptr tf2_broadcaster_; + std::mutex mt; + rclcpp::Service::SharedPtr service_; + std::string assemble_dir_; + std::string assemble_prefix_; +}; + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(AssembleStateServer); diff --git a/rbs_skill_servers/src/pick_place_pose_loader.cpp b/rbs_skill_servers/src/pick_place_pose_loader.cpp index 755fbe2..423f983 100644 --- a/rbs_skill_servers/src/pick_place_pose_loader.cpp +++ b/rbs_skill_servers/src/pick_place_pose_loader.cpp @@ -24,7 +24,7 @@ void GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request, rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response) { - std::string rrr = request->object_name + "_place"; // TODO: replace with better name + std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name try { tf_data = tf_buffer_->lookupTransform( "world", rrr.c_str(), @@ -107,4 +107,4 @@ GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector pose) aff.translation() = point; aff.linear() = quat.toRotationMatrix(); return aff; -} \ No newline at end of file +} diff --git a/rbs_task_planner/README.md b/rbs_task_planner/README.md new file mode 100644 index 0000000..bed9017 --- /dev/null +++ b/rbs_task_planner/README.md @@ -0,0 +1,35 @@ +### Usage +change `rbs_simulation.laucnh.py` file by providing `${PATH}` and `${PREFIX}` to assemble files +```python + assemble_state = Node( + package="rbs_skill_servers", + executable="assemble_state_service_server", + output="screen", + emulate_tty=True, + parameters=[ + {"assemble_prefix": "ASSEMBLE_"} # ${PREFIX} + {"assemble_package_dir": ${COLCON_WS}/src/robossembler-ros2/rbs_task_planner/exemples/sdf_models}, # ${PATH} + ] + ) +``` +launch simulation proccess +```bash +ros2 launch rbs_simulation rbs_simulation.launch.py +``` +launch task_planner process +```bash +ros2 launch rbs_task_planner task_planner.launch.py +``` +start env_manager process +```bash +./${COLCON_WS}/build/env_manager/main +``` +you can send `problem` throw plansys2 service with `${PROBLEM_FILE_CONTENT}` +```bash +ros2 service call /problem_expert/add_problem plansys2_msgs/srv/AddProblem '{problem: ${PROBLEM_FILE_CONTENT}}' +``` +or you can use `plansys2_terminal` +```bash +ros2 run plansys2_terminal plansys2_terminal +``` +after that use `plansys2_terminal` to call `run` command and start executing process diff --git a/rbs_task_planner/config/params.yaml b/rbs_task_planner/config/params.yaml index e69de29..4659273 100644 --- a/rbs_task_planner/config/params.yaml +++ b/rbs_task_planner/config/params.yaml @@ -0,0 +1,10 @@ +assemble: + ros__parameters: + plugins: + - rbs_skill_move_topose_bt_action_client + - rbs_skill_get_pick_place_pose_service_client + - rbs_skill_gripper_move_bt_action_client + - rbs_skill_move_joint_state + - rbs_assemble_process_state + - rbs_detect_object + diff --git a/rbs_task_planner/domain/atomic_domain.pddl b/rbs_task_planner/domain/atomic_domain.pddl index 3a511b1..3cbef21 100644 --- a/rbs_task_planner/domain/atomic_domain.pddl +++ b/rbs_task_planner/domain/atomic_domain.pddl @@ -1,7 +1,7 @@ (define (domain atomic_domain) (:requirements :strips :typing :adl :fluents :durative-actions) (:types - workspace - zone + zone part arm assembly @@ -19,16 +19,17 @@ :parameters (?p - part ?prev ?next - assembly ?z - zone ?a - arm) :duration ( = ?duration 1) :condition (and - (at start (assembly_order ?prev ?next)) - (at start (assembled ?prev)) - (at start (assembly_at ?prev ?z)) - (at start (part_of ?p ?next)) (at start (arm_available ?a)) + (at start (assembly_order ?prev ?next)) + (at start (part_of ?p ?next)) + (at start (assembly_at ?prev ?z)) + (at start (assembled ?prev)) ) :effect (and (at start (not (arm_available ?a))) (at end (assembly_at ?next ?z)) (at end (assembled ?next)) + (at end (arm_available ?a)) ) ) ) diff --git a/rbs_task_planner/example/sdf_models/asm1.sdf b/rbs_task_planner/example/sdf_models/asm1.sdf new file mode 100644 index 0000000..4e64b2f --- /dev/null +++ b/rbs_task_planner/example/sdf_models/asm1.sdf @@ -0,0 +1,19 @@ + + + + + 0 0 0 0 0 0 + + + model://box1 + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + base_link + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + + diff --git a/rbs_task_planner/example/sdf_models/asm2.sdf b/rbs_task_planner/example/sdf_models/asm2.sdf new file mode 100644 index 0000000..b0d3d9a --- /dev/null +++ b/rbs_task_planner/example/sdf_models/asm2.sdf @@ -0,0 +1,28 @@ + + + + + 0 0 0 0 0 0 + + + model://box1 + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box2 + -0.45 0.0 0.46 -3.1415927 0.0 0.0 + + + base_link + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + base_link + box2 + -0.45 0.0 0.46 -3.1415927 0.0 0.0 + + + diff --git a/rbs_task_planner/example/sdf_models/asm3.sdf b/rbs_task_planner/example/sdf_models/asm3.sdf new file mode 100644 index 0000000..8e9f7f4 --- /dev/null +++ b/rbs_task_planner/example/sdf_models/asm3.sdf @@ -0,0 +1,38 @@ + + + + + 0 0 0 0 0 0 + + + model://box1 + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box2 + -0.45 0.0 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box3 + -0.45 0.055 0.46 -3.1415927 0.0 0.0 + + + base_link + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + base_link + box2 + -0.45 0.0 0.46 -3.1415927 0.0 0.0 + + + base_link + box3 + -0.45 0.055 0.46 -3.1415927 0.0 0.0 + + + diff --git a/rbs_task_planner/example/sdf_models/asm4.sdf b/rbs_task_planner/example/sdf_models/asm4.sdf new file mode 100644 index 0000000..d1a984f --- /dev/null +++ b/rbs_task_planner/example/sdf_models/asm4.sdf @@ -0,0 +1,48 @@ + + + + + 0 0 0 0 0 0 + + + model://box1 + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box2 + -0.45 0.0 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box3 + -0.45 0.055 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box4 + -0.45 0.0275 0.49 -3.1415927 0.0 0.0 + + + base_link + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + base_link + box2 + -0.45 0.0 0.46 -3.1415927 0.0 0.0 + + + base_link + box3 + -0.45 0.055 0.46 -3.1415927 0.0 0.0 + + + base_link + box4 + -0.45 0.0275 0.49 -3.1415927 0.0 0.0 + + + diff --git a/rbs_task_planner/example/sdf_models/asm5.sdf b/rbs_task_planner/example/sdf_models/asm5.sdf new file mode 100644 index 0000000..56e831e --- /dev/null +++ b/rbs_task_planner/example/sdf_models/asm5.sdf @@ -0,0 +1,58 @@ + + + + + 0 0 0 0 0 0 + + + model://box1 + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box2 + -0.45 0.0 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box3 + -0.45 0.055 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box4 + -0.45 0.0275 0.49 -3.1415927 0.0 0.0 + + + model://box1 + box5 + -0.45 -0.0275 0.49 -3.1415927 0.0 0.0 + + + base_link + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + base_link + box2 + -0.45 0.0 0.46 -3.1415927 0.0 0.0 + + + base_link + box3 + -0.45 0.055 0.46 -3.1415927 0.0 0.0 + + + base_link + box4 + -0.45 0.0275 0.49 -3.1415927 0.0 0.0 + + + base_link + box5 + -0.45 -0.0275 0.49 -3.1415927 0.0 0.0 + + + diff --git a/rbs_task_planner/example/sdf_models/asm6.sdf b/rbs_task_planner/example/sdf_models/asm6.sdf new file mode 100644 index 0000000..08f28fd --- /dev/null +++ b/rbs_task_planner/example/sdf_models/asm6.sdf @@ -0,0 +1,68 @@ + + + + + 0 0 0 0 0 0 + + + model://box1 + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box2 + -0.45 0.0 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box3 + -0.45 0.055 0.46 -3.1415927 0.0 0.0 + + + model://box1 + box4 + -0.45 0.0275 0.49 -3.1415927 0.0 0.0 + + + model://box1 + box5 + -0.45 -0.0275 0.49 -3.1415927 0.0 0.0 + + + model://box1 + box6 + -0.45 0.0 0.56 -3.1415927 0.0 0.0 + + + base_link + box1 + -0.45 -0.055 0.46 -3.1415927 0.0 0.0 + + + base_link + box2 + -0.45 0.0 0.46 -3.1415927 0.0 0.0 + + + base_link + box3 + -0.45 0.055 0.46 -3.1415927 0.0 0.0 + + + base_link + box4 + -0.45 0.0275 0.49 -3.1415927 0.0 0.0 + + + base_link + box5 + -0.45 -0.0275 0.49 -3.1415927 0.0 0.0 + + + base_link + box6 + -0.45 0.0 0.56 -3.1415927 0.0 0.0 + + + diff --git a/rbs_task_planner/example/sdf_models/cat_problem b/rbs_task_planner/example/sdf_models/cat_problem new file mode 100644 index 0000000..11a7c96 --- /dev/null +++ b/rbs_task_planner/example/sdf_models/cat_problem @@ -0,0 +1 @@ +(define (problem box_problem)(:domain atomic_domain)(:objects ur_manipulator_gripper - arm world - zone box1 box2 box3 box4 box5 box6 - part asm0 asm1 asm2 asm3 asm4 asm5 asm6 - assembly)(:init (part_of box1 asm1) (part_of box2 asm2) (part_of box3 asm3) (part_of box4 asm4) (part_of box5 asm5) (part_of box6 asm6) (assembly_order asm0 asm1) (assembly_order asm1 asm2) (assembly_order asm2 asm3) (assembly_order asm3 asm4) (assembly_order asm4 asm5) (assembly_order asm5 asm6) (arm_available ur_manipulator_gripper) (assembly_at asm0 world) (assembled asm0) )(:goal (and (assembled asm6)))) diff --git a/rbs_task_planner/example/sdf_models/problem.pddl b/rbs_task_planner/example/sdf_models/problem.pddl new file mode 100644 index 0000000..4008176 --- /dev/null +++ b/rbs_task_planner/example/sdf_models/problem.pddl @@ -0,0 +1,26 @@ +(define (problem box_problem) +(:domain atomic_domain) +(:objects + ur_manipulator_gripper - arm + workspace1 - zone + box1 box2 box3 - part + asm0 asm1 asm2 asm3 - assembly +) + +(:init + (part_of box1 asm1) + (part_of box2 asm2) + (part_of box3 asm3) + (assembly_order asm0 asm1) + (assembly_order asm1 asm2) + (assembly_order asm2 asm3) + + (arm_available ur_manipulator_gripper) + (assembly_at asm0 workspace1) + (assembled asm0) +) + +(:goal (and + (assembled asm3) +)) +) diff --git a/rbs_task_planner/launch/task_planner.launch.py b/rbs_task_planner/launch/task_planner.launch.py index 85897da..074c3d8 100644 --- a/rbs_task_planner/launch/task_planner.launch.py +++ b/rbs_task_planner/launch/task_planner.launch.py @@ -6,7 +6,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -# from launch_ros.actions import Node +from launch_ros.actions import Node def load_file(package_name, file_path): @@ -36,6 +36,7 @@ def load_yaml(package_name, file_path): def generate_launch_description(): pkg_dir = get_package_share_directory('rbs_task_planner') + bt_exec_dir = get_package_share_directory('rbs_bt_executor') namespace = LaunchConfiguration('namespace') declare_namespace_cmd = DeclareLaunchArgument( @@ -52,7 +53,6 @@ def generate_launch_description(): 'model_file': pkg_dir + '/domain/atomic_domain.pddl', 'namespace': namespace }.items()) - """ assemble = Node( package='plansys2_bt_actions', executable='bt_action_node', @@ -61,20 +61,16 @@ def generate_launch_description(): output='screen', parameters=[ pkg_dir + '/config/params.yaml', - robot_description, - robot_description_semantic, - kinematics_yaml, { 'action_name': 'assemble', - # 'publisher_port': 1666, - # 'server_port': 1667, - 'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/assemble.xml', + 'bt_xml_file': bt_exec_dir + '/bt_trees/assemble.xml', } ]) - """ ld = LaunchDescription() ld.add_action(declare_namespace_cmd) # Declare the launch options ld.add_action(plansys2_cmd) + ld.add_action(assemble) + return ld diff --git a/rbs_task_planner/package.xml b/rbs_task_planner/package.xml index 7c28199..600e955 100644 --- a/rbs_task_planner/package.xml +++ b/rbs_task_planner/package.xml @@ -18,9 +18,12 @@ plansys2_planner plansys2_executor plansys2_bt_actions + plansys2_terminal behaviortree_cpp_v3 + behavior_tree popf + plansys2_bringup ament_lint_common ament_lint_auto diff --git a/rbs_task_planner/problems/example_problem.pddl b/rbs_task_planner/problems/example_problem.pddl new file mode 100644 index 0000000..8beb48a --- /dev/null +++ b/rbs_task_planner/problems/example_problem.pddl @@ -0,0 +1,32 @@ +(define (problem box_problem) +(:domain atomic_domain) +(:objects + ur_manipulator_gripper - arm + world - zone + box1 box2 box3 box4 box5 box6 - part + asm0 asm1 asm2 asm3 asm4 asm5 asm6 - assembly +) + +(:init + (part_of box1 asm1) + (part_of box2 asm2) + (part_of box3 asm3) + (part_of box4 asm4) + (part_of box5 asm5) + (part_of box6 asm6) + (assembly_order asm0 asm1) + (assembly_order asm1 asm2) + (assembly_order asm2 asm3) + (assembly_order asm3 asm4) + (assembly_order asm4 asm5) + (assembly_order asm5 asm6) + + (arm_available ur_manipulator_gripper) + (assembly_at asm0 world) + (assembled asm0) +) + +(:goal (and + (assembled asm6) +)) +) diff --git a/rbs_task_planner/problems/problem.pddl b/rbs_task_planner/problems/problem.pddl index f426473..e089fcf 100644 --- a/rbs_task_planner/problems/problem.pddl +++ b/rbs_task_planner/problems/problem.pddl @@ -1,30 +1,31 @@ -(define (problem cube3-problem) +(define (problem Cube3-problem) (:domain atomic_domain) (:objects - ws1 - zone - rbsarm - arm - cube3 cube1 cube2 cube004 cube005 cube006 cube007 - part - zeroasm subasm0 subasm1 subasm2 subasm3 subasm4 subasm5 subasm6 - assembly + ur_manipulator_gripper - arm + ws - zone + cube cube001 cube002 cube004 cube005 cube006 - part + asm1 asm2 asm3 asm4 asm5 asm6 - assembly ) (:init - (part_of cube3 subasm0) - (part_of cube1 subasm1) - (part_of cube2 subasm2) - (part_of cube004 subasm3) - (part_of cube005 subasm4) - (part_of cube006 subasm5) - (part_of cube007 subasm6) - (assembly_order zeroasm subasm0) - (assembly_order subasm0 subasm1) - (assembly_order subasm1 subasm2) - (assembly_order subasm3 subasm4) - (assembly_order subasm5 subasm6) + (part_of cube asm1) + (part_of cube001 asm2) + (part_of cube002 asm3) + (part_of cube004 asm4) + (part_of cube005 asm5) + (part_of cube006 asm6) + (assembly_order asm1 asm2) + (assembly_order asm2 asm3) + (assembly_order asm3 asm4) + (assembly_order asm4 asm5) + (assembly_order asm5 asm6) - (arm_available rbsarm) - (assembly_at zeroasm ws1) - (assembled zeroasm) + (arm_available ur_manipulator_gripper) + (assembly_at asm1 ws) + (assembled asm1) ) -(:goal (and (assembled subasm6))) +(:goal (and + (assembled asm6) +)) )