⚡ add EmuPoseEstimation and improve Readme.md

README.md

@@ -1,3 +1,59 @@
 # Robossembler ROS2
 
-Repo for ROS2 packages related to Robossembler
+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)

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)
 

rasms_manipulator/README.md

@@ -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)

rasms_manipulator/behavior_trees_xml/EmuPoseEstimation.xml

@@ -0,0 +1,7 @@
+<root main_tree_to_execute = "MainTree" >
+    <BehaviorTree ID="MainTree">
+       <Sequence name="root_sequence">
+           <Move name="EmuPoseEstimation" goal="${arg1}"/>
+       </Sequence>
+    </BehaviorTree>
+</root>

rasms_manipulator/include/rasms_bt/behavior_tree_nodes/EmuPose_Estimation.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <string>
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include <gazebo_msgs/srv/get_entity_state.hpp>
+#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<std::string>("model_name")};
+        }
+
+private:
+};

rasms_manipulator/src/behavior_tree_nodes/EmuPoseEstimation.cpp

@@ -0,0 +1,15 @@
+#include <iostream>
+#include <rclcpp/executors/single_threaded_executor.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/qos.hpp>
+#include <string>
+#include <thread>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+
+//#include "rasms_bt/behavior_tree_nodes/EmuPoseEstimation.hpp"
+
+
+int main(int argc, char * argv[])
+{
+        return 0;
+}