#6: try to build node

rasms_manipulator/.vscode/c_cpp_properties.json

@@ -0,0 +1,22 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "",
+        "limitSymbolsToIncludedHeaders": true
+      },
+      "includePath": [
+        "/home/splinter1984/robas_ws/install/moveit_visual_tools/include/**",
+        "/home/splinter1984/robas_ws/install/rviz_visual_tools/include/**",
+        "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph/include/**",
+        "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph_msgs/include/**",
+        "/opt/ros/foxy/include/**",
+        "/home/splinter1984/robas_ws/src/moveit_visual_tools/include/**",
+        "/home/splinter1984/robas_ws/src/rviz_visual_tools/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS"
+    }
+  ],
+  "version": 4
+}

rasms_manipulator/.vscode/settings.json

@@ -0,0 +1,14 @@
+{
+    "cmake.configureOnOpen": true,
+    "python.autoComplete.extraPaths": [
+        "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph_viewer/lib/python3.8/site-packages",
+        "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph_msgs/lib/python3.8/site-packages",
+        "/opt/ros/foxy/lib/python3.8/site-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph_viewer/lib/python3.8/site-packages",
+        "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph_msgs/lib/python3.8/site-packages",
+        "/opt/ros/foxy/lib/python3.8/site-packages"
+    ],
+    "ros.distro": "foxy"
+}

rasms_manipulator/behavior_trees/move.xml

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

rasms_manipulator/config/params.yaml

@@ -0,0 +1,9 @@
+move_1:
+  ros_parameters:
+    plugins:
+      - rasms_move_bt_node
+    waypoints: ["zero", "one"]
+    waypoints_coords:
+      zero: [0.0, -2.0, 0.0]
+      one: [1.8, 0.0, 0.0]
+      

rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp

@@ -0,0 +1,64 @@
+#pragma once
+
+#include <string>
+#include <map>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "geometry_msgs/msg/pose2_d.hpp"
+
+#include "plansys2_bt_actions/BTActionNode.hpp"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "moveit_msgs/msg/place_location.hpp"
+#include "moveit_msgs/msg/move_it_error_codes.hpp"
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2/transform_datatypes.h"
+#include "tf2/LinearMath/Transform.h"
+#include "tf2_msgs/msg/tf_message.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/create_timer_ros.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/moveit_cpp/planning_component.h>
+
+#include <geometry_msgs/msg/point_stamped.h>
+
+namespace rasms_manipulation
+{
+class Move: public BT::ActionNodeBase
+{
+public:
+    explicit Move(
+        const std::string& xml_tag_name,
+        const BT::NodeConfiguration& conf);
+    
+    void halt();
+    BT::NodeStatus on_tick();
+
+    geometry_msgs::msg::PoseStamped pose2BaseFootprint(geometry_msgs::msg::PoseStamped input);
+    void resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg);
+
+    static BT::PortsList providedPorts()
+    {
+        return {
+            BT::InputPort<geometry_msgs::msg::PoseStamped>("goal")
+        };
+    }
+
+private:
+    rclcpp::Node::SharedPtr node_;
+    rclcpp::Publisher<moveit_msgs::msg::PlaceLocation>::SharedPtr move_pub_;
+    rclcpp::Subscription<moveit_msgs::msg::MoveItErrorCodes>::SharedPtr result_sub_;
+    std::map<std::string, geometry_msgs::msg::Point> places_;
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+    int result_;
+    bool move_action_sent_;
+};
+
+} //namespace rasms_manipulator

rasms_manipulator/src/behavior_tree_nodes/Move.cpp

@@ -0,0 +1,115 @@
+#include <string>
+#include <iostream>
+
+#include "rasms_bt/behavior_tree_nodes/Move.hpp"
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+namespace rasms_manipulation
+{
+Move::Move(
+    const std::string& xml_tag_name, 
+    const BT::NodeConfiguration& conf)
+    : BT::ActionNodeBase(xml_tag_name, conf)
+{
+    node_ = rclcpp::Node::make_shared("move_action_comms");
+    move_pub_ = node_->create_publisher<moveit_msgs::msg::PlaceLocation>("/moveit/place", 1);
+    result_sub_ = node_->create_subscription<moveit_msgs::msg::MoveItErrorCodes>(
+        "/moveit/result", 
+        1,
+        std::bind(&Move::resultCallback, this, std::placeholders::_1));
+    rclcpp::Node::SharedPtr bt_node;
+    config().blackboard->get("node", bt_node);
+
+    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+        node_->get_node_base_interface(),
+        node_->get_node_timers_interface());
+    tf_buffer_->setCreateTimerInterface(timer_interface);
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+    move_action_sent_ = false;
+}
+
+void Move::halt()
+{
+    std::cout << "Move halt" << std::endl;
+}
+
+void Move::resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg)
+{
+    result_ = msg->val;
+}
+
+double getYaw(geometry_msgs::msg::Quaternion mQ)
+{
+    tf2::Quaternion tQ(
+        mQ.x,
+        mQ.y,
+        mQ.z,
+        mQ.w);
+    tf2::Matrix3x3 m(tQ);
+    double roll, pitch, yaw;
+    m.getRPY(roll, pitch, yaw);
+    return yaw;
+}
+
+geometry_msgs::msg::PoseStamped Move::pose2BaseFootprint(geometry_msgs::msg::PoseStamped input)
+{
+    tf2::Transform frame2bf, obj2frame, bf2Object;
+    obj2frame.setOrigin({input.pose.posetion.x,
+                        input.pose.position.y,
+                        input.pose.position.z});
+    obj2frame.setRotation({input.pose.posetion.x,
+                        input.pose.position.y,
+                        input.pose.position.z,
+                        input.pose.position.w});
+    geometry_msgs::msg::PoseStamped object_pose;
+    try
+    {
+        auto tf = tf_buffer_->lookupTransform(
+            input.header.frame_id, "base", tf2::TimePointZero);
+        tf2::fromMsg(tf.transform, frame2bf);
+        bf2Object = obj2frame * frame2bf;
+    }
+}
+
+BT::NodeStatus Move::on_tick()
+{
+    if (!move_action_sent_)
+    {
+        geometry_msgs::msg::PoseStamped goal;
+        getInput<geometry_msgs::msg::PoseStamped>("goal", goal);
+        RCLCPP_INFO(node_->get_logger(), "Move to: x:%f y:%f z:%f yaw:%f",
+            goal.pose.position.x,
+            goal.pose.position.y,
+            goal.pose.position.z,
+            getYaw(goal.pose.orientation));
+        moveit_msgs::msg::PlaceLocation msg;
+        msg.place_pose = pose2BaseFootprint(goal);
+        move_pub_->publish(msg);
+        result_ = 0;
+        move_action_sent_ = true;
+    }
+
+    rclcpp:spin_some(node_);
+
+    if (result_ == 0)
+    {
+        return BT::NodeStatus::RUNNING;
+    } else if (result_ == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
+    {
+        result_ = 0;
+        RCLCPP_INFO(node_->get_logger(), "Success!");
+        return BT::NodeStatus::SUCCESS;
+    } else {
+        RCLCPP_ERROR(node_->get_logger(), "Move error: MoveItErrorCodes[%i]", result_);
+        return BT::NodeStatus::FAILURE;
+    }
+}
+} //namespace rasms_manipulator
+
+BT_REGISTER_NODES(factory)
+{
+    factory.registerNodeType<rasms_manipulation::Move>("Move");
+}

rasms_manipulator/src/rasms_controller_node.cpp

@@ -0,0 +1,110 @@
+#include <memory>
+#include <random>
+
+#include "plansys2_msgs/msg/action_execution_info.hpp"
+
+#include "plansys2_executor/ExecutorClient.hpp"
+#include "plansys2_domain_expert/DomainExpertClient.hpp"
+#include "plansys2_problem_expert/ProblemExpertClient.hpp"
+#include "plansys2_planner/PlannerClient.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+class RasmsController: public rclcpp::Node
+{
+public:
+    RasmsController(): rclcpp::Node("rasms_controller")
+    {}
+
+    void init()
+    {
+        domain_expert_ = std::make_shared<plansys2::DomainExpertClient>();
+        problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>();
+        executor_client_ = std::make_shared<plansys2::ExecutorClient>();
+        planner_client_ = std::make_shared<plansys2::PlannerClient>();
+
+        init_knowledge();
+
+        auto domain = domain_expert_->getDomain();
+        auto problem = problem_expert_->getProblem();
+        auto plan = planner_client_->getPlan(domain, problem);
+
+        if (plan.has_value())
+        {
+            if (!executor_client_->start_plan_execution(plan.value()))
+            {
+                RCLCPP_ERROR(get_logger(), "Error starting a new plan (first)");
+            }
+        } else {
+            RCLCPP_ERROR_STREAM(
+                this->get_logger(), "Could not find plan to reach goal " <<
+                parser::pddl::toString(problem_expert_->getGoal()));
+        }
+    }
+
+    void init_knowledge()
+    {
+        problem_expert_->addInstance(plansys2::Instance{"rasms", "robot"});
+        problem_expert_->setGoal(plansys2::Goal("robot moved"));
+    }
+
+    std::string status_to_string(int8_t status) 
+    {
+        switch (status)
+        {
+        case plansys2_msgs::msg::ActionExecutionInfo::NOT_EXECUTED:
+            return "NOT_EXECUTED";
+            break;
+        case plansys2_msgs::msg::ActionExecutionInfo::EXECUTING:
+            return "EXECUTING";
+            break;
+        case plansys2_msgs::msg::ActionExecutionInfo::FAILED:
+            return "FAILED";
+            break;
+        case plansys2_msgs::msg::ActionExecutionInfo::SUCCEEDED:
+            return "SUCCEEDED";
+            break;
+        case plansys2_msgs::msg::ActionExecutionInfo::CANCELLED:
+            return "CANCELLED";
+            break;
+        default:
+            return "UNKNOWN";
+            break;
+        }
+    }
+
+    void step()
+    {
+        if (!executor_client_->execute_and_check_plan()) 
+        {
+            RCLCPP_INFO(get_logger(), "Done!");
+        }
+    }
+
+private:
+    std::shared_ptr<plansys2::ProblemExpertClient> problem_expert_;
+    std::shared_ptr<plansys2::DomainExpertClient> domain_expert_;
+    std::shared_ptr<plansys2::ExecutorClient> executor_client_;
+    std::shared_ptr<plansys2::PlannerClient> planner_client_;
+};
+
+int main(argc, char ** argv)
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<RasmsController>();
+
+    node->init();
+
+    rclcpp::Rate rate(1);
+    while (rclcpp::ok())
+    {
+        rate.sleep();
+        rclcpp::spin_some(node->get_node_base_interface());
+        node->step();
+    }
+
+    rclcpp::shutdown();
+
+    return 0;
+}