From 5a78aa95382931103268ccb069727af07eb5c1a2 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Wed, 12 Jan 2022 21:14:36 +0400 Subject: [PATCH] :zap: add EmuPoseEstimation and improve Readme.md --- README.md | 58 ++++++++++++++++++- rasms_manipulator/CMakeLists.txt | 4 ++ rasms_manipulator/README.md | 53 ----------------- .../behavior_trees_xml/EmuPoseEstimation.xml | 7 +++ .../EmuPose_Estimation.hpp | 29 ++++++++++ .../behavior_tree_nodes/EmuPoseEstimation.cpp | 15 +++++ 6 files changed, 112 insertions(+), 54 deletions(-) delete mode 100644 rasms_manipulator/README.md create mode 100644 rasms_manipulator/behavior_trees_xml/EmuPoseEstimation.xml create mode 100644 rasms_manipulator/include/rasms_bt/behavior_tree_nodes/EmuPose_Estimation.hpp create mode 100644 rasms_manipulator/src/behavior_tree_nodes/EmuPoseEstimation.cpp diff --git a/README.md b/README.md index 06024ef..11f63ac 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,59 @@ # Robossembler ROS2 -Repo for ROS2 packages related to Robossembler \ No newline at end of file +Repo for ROS2 packages related to Robossembler + +### Install + +1. Clone the repository +2. Build packages + +For visualization install ```colcon``` with ```mixin``` (it's required to install ```moveit_visual_tools```): +```bash +sudo apt install python3-colcon-common-extensions python3-colcon-mixin python3-vcstool +colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml +colcon mixin update default +``` + +Prepare workspace & install dependencies +``` +mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src +git clone https://gitlab.com/robosphere/robossembler-ros2 -b dev-motion-planning-plansys2 +vcs import . < robossembler-ros2/rasms.repos +cd .. +rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy +colcon build --symlink-install --mixin release +``` + +but at the moment the visualization is not implemented + +### Run +Add source to environment +``` +source install/setup.bash +``` +Launch MoveIt2, Gazebo, RViz +```bash +ros2 launch rasms_moveit_config rasms_bringup.launch.py +``` +Launch PlanSys2 with domain from ```pddl/domain.pddl``` +```bash +ros2 launch rasms_manipulator rasms_manipulation.launch.py +``` +Launch Plansys2 Terminal +```bash +ros2 run plansys2_terminal plansys2_terminal +``` +Then into plansys2_terminal paste command (see updates into pddl/commands) +```bash +set instance rasms robot +set instance one zone +set instance two zone +set predicate (robot_at rasms one) +set goal (and(robot_at rasms two)) +run +``` + +### Links +* [plansys2_bt](https://intelligentroboticslab.gsyc.urjc.es/ros2_planning_system.github.io/tutorials/docs/bt_actions.html) +* [bt_v3_cpp](https://www.behaviortree.dev) +* [moveit2](https://moveit.picknik.ai/foxy/index.html) \ No newline at end of file diff --git a/rasms_manipulator/CMakeLists.txt b/rasms_manipulator/CMakeLists.txt index 7aecb87..6490400 100644 --- a/rasms_manipulator/CMakeLists.txt +++ b/rasms_manipulator/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(plansys2_problem_expert REQUIRED) find_package(plansys2_pddl_parser REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(plansys2_bt_actions REQUIRED) +find_package(gazebo_msgs REQUIRED) if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -39,10 +40,13 @@ set(dependencies plansys2_pddl_parser ament_index_cpp plansys2_bt_actions + gazebo_msgs ) include_directories(include) +#add_library(rasmsPoseEstimation src/EmuPoseEstimation.cpp) + add_library(rasms_move_bt_node SHARED src/behavior_tree_nodes/Move.cpp) list(APPEND plugin_libs rasms_move_bt_node) diff --git a/rasms_manipulator/README.md b/rasms_manipulator/README.md deleted file mode 100644 index 23d452c..0000000 --- a/rasms_manipulator/README.md +++ /dev/null @@ -1,53 +0,0 @@ -# Robossembler Plansys2 example - -Plansys2 & MoveIt2 example on ROS2 Foxy - -### Install -Install ```colcon``` with ```mixin``` (it's required to install ```moveit_visual_tools```): -```bash -sudo apt install python3-colcon-common-extensions python3-colcon-mixin python3-vcstool -colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml -colcon mixin update default -``` - -Prepare workspace & install dependencies -``` -mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src -git clone https://gitlab.com/robosphere/robossembler-ros2 -b dev-motion-planning-plansys2 -vcs import . < robossembler-ros2/rasms.repos -cd .. -rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy -colcon build --symlink-install --mixin release -``` - -### Run -Add source to environment -``` -source install/setup.bash -``` -Launch MoveIt2, Gazebo, RViz -```bash -ros2 launch rasms_moveit_config rasms_bringup.launch.py -``` -Launch PlanSys2 with domain from ```pddl/domain.pddl``` -```bash -ros2 launch rasms_manipulator rasms_manipulation.launch.py -``` -Launch Plansys2 Terminal -```bash -ros2 run plansys2_terminal plansys2_terminal -``` -Then into plansys2_terminal paste command (see updates into pddl/commands) -```bash -set instance rasms robot -set instance one zone -set instance two zone -set predicate (robot_at rasms one) -set goal (and(robot_at rasms two)) -run -``` - -### Links -* [plansys2_bt](https://intelligentroboticslab.gsyc.urjc.es/ros2_planning_system.github.io/tutorials/docs/bt_actions.html) -* [bt_v3_cpp](https://www.behaviortree.dev) -* [moveit2](https://moveit.picknik.ai/foxy/index.html) diff --git a/rasms_manipulator/behavior_trees_xml/EmuPoseEstimation.xml b/rasms_manipulator/behavior_trees_xml/EmuPoseEstimation.xml new file mode 100644 index 0000000..478886d --- /dev/null +++ b/rasms_manipulator/behavior_trees_xml/EmuPoseEstimation.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/EmuPose_Estimation.hpp b/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/EmuPose_Estimation.hpp new file mode 100644 index 0000000..25b9c32 --- /dev/null +++ b/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/EmuPose_Estimation.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include +#include "plansys2_bt_actions/BTActionNode.hpp" +#include "rclcpp/rclcpp.hpp" + + + +class EmuPoseEstimation : BT::ActionNodeBase +{ +public: + EmuPoseEstimation(const std::string &xml_tag, + const BT::NodeConfiguration &conf); + void resultCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + + virtual void halt() override; + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return {BT::InputPort("model_name")}; + } + +private: +}; \ No newline at end of file diff --git a/rasms_manipulator/src/behavior_tree_nodes/EmuPoseEstimation.cpp b/rasms_manipulator/src/behavior_tree_nodes/EmuPoseEstimation.cpp new file mode 100644 index 0000000..6df2d33 --- /dev/null +++ b/rasms_manipulator/src/behavior_tree_nodes/EmuPoseEstimation.cpp @@ -0,0 +1,15 @@ +#include +#include +#include +#include +#include +#include +#include + +//#include "rasms_bt/behavior_tree_nodes/EmuPoseEstimation.hpp" + + +int main(int argc, char * argv[]) +{ + return 0; +}