intialize Move

rasms_manipulator/.vscode/settings.json

@@ -11,5 +11,14 @@
         "/opt/ros/foxy/lib/python3.8/site-packages"
     ],
     "ros.distro": "foxy",
-    "C_Cpp.errorSquiggles": "Disabled"
+    "C_Cpp.errorSquiggles": "Disabled",
+    "files.associations": {
+        "unordered_map": "cpp",
+        "unordered_set": "cpp"
+    },
+    "editor.maxTokenizationLineLength": 2000,
+    "editor.peekWidgetDefaultFocus": "editor",
+    "editor.find.seedSearchStringFromSelection": "never",
+    "diffEditor.maxComputationTime": 500,
+    "diffEditor.maxFileSize": 25
 }

rasms_manipulator/behavior_trees_xml/move.xml

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

rasms_manipulator/config/params.yaml

@@ -1,7 +1,8 @@
-move:
+move_1:
   ros_parameters:
-    waypoints: ["zero", "one"]
+    plugins:
+      - plansys2_move_bt_node
+    waypoints: ["zero"]
     waypoints_coords:
-      zero: [0.0, -2.0, 0.0]
-      one: [1.8, 0.0, 0.0]
+      zero: [0.0, -2.0, 0.0, 0.0]
       

rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp

@@ -14,32 +14,24 @@
 #include "moveit_msgs/msg/place_location.hpp"
 #include "moveit_msgs/msg/move_it_error_codes.hpp"
 #include "moveit_msgs/action/move_group.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_msgs/action/"
 
 #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
+class Move: public plansys2::BtActionNode<moveit_msgs::MoveGroupAction>
 {
 public:
     explicit Move(
         const std::string& xml_tag_name,
+        const std::string& action_name,
         const BT::NodeConfiguration& conf);
     
-    void halt();
-    BT::NodeStatus on_tick();
+    void on_tick() override;
+    BT::NodeStatus on_success() override;
 
     geometry_msgs::msg::PoseStamped pose2BaseFootprint(geometry_msgs::msg::PoseStamped input);
     void resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg);
@@ -47,19 +39,13 @@ public:
     static BT::PortsList providedPorts()
     {
         return {
-            BT::InputPort<geometry_msgs::msg::PoseStamped>("goal")
+            BT::InputPort<std::string>("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_;
+    int goal_reached_;
+    std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
 };
 
 } //namespace rasms_manipulator

rasms_manipulator/src/assemble_action_node.cpp

@@ -0,0 +1,68 @@
+// Copyright 2019 Intelligent Robotics Lab
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <algorithm>
+
+#include "plansys2_executor/ActionExecutorClient.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+using namespace std::chrono_literals;
+
+class AssembleAction : public plansys2::ActionExecutorClient
+{
+public:
+  AssembleAction()
+  : plansys2::ActionExecutorClient("assemble", 500ms)
+  {
+    progress_ = 0.0;
+  }
+
+private:
+  void do_work()
+  {
+    if (progress_ < 1.0) {
+      progress_ += 0.05;
+      send_feedback(progress_, "Assemble running");
+    } else {
+      finish(true, 1.0, "Assemble completed");
+
+      progress_ = 0.0;
+      std::cout << std::endl;
+    }
+
+    std::cout << "\r\e[K" << std::flush;
+    std::cout << "Assemble ... [" << std::min(100.0, progress_ * 100.0) << "%]  " <<
+      std::flush;
+  }
+
+  float progress_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<AssembleAction>();
+
+  node->set_parameter(rclcpp::Parameter("action_name", "assemble"));
+  node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+
+  rclcpp::spin(node->get_node_base_interface());
+
+  rclcpp::shutdown();
+
+  return 0;
+}

rasms_manipulator/src/assemble_controller_node.cpp

@@ -0,0 +1,116 @@
+// Copyright 2019 Intelligent Robotics Lab
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <plansys2_pddl_parser/Utils.h>
+
+#include <memory>
+
+#include "plansys2_msgs/msg/action_execution_info.hpp"
+#include "plansys2_msgs/msg/plan.hpp"
+
+#include "plansys2_domain_expert/DomainExpertClient.hpp"
+#include "plansys2_executor/ExecutorClient.hpp"
+#include "plansys2_planner/PlannerClient.hpp"
+#include "plansys2_problem_expert/ProblemExpertClient.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+class Assemble : public rclcpp::Node
+{
+public:
+    Assemble()
+    : rclcpp::Node("assembling_controller")
+    {
+    }
+
+    bool init()
+    {
+        domain_expert_ = std::make_shared<plansys2::DomainExpertClient>();
+        planner_client_ = std::make_shared<plansys2::PlannerClient>();
+        problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>();
+        executor_client_ = std::make_shared<plansys2::ExecutorClient>();
+
+        //init_knowledge();
+
+        auto domain = domain_expert_->getDomain();
+        auto problem = problem_expert_->getProblem();
+        auto plan = planner_client_->getPlan(domain, problem);
+
+        if (!plan.has_value()) 
+        {
+            std::cout << "Could not find plan to reach goal " <<
+                parser::pddl::toString(problem_expert_->getGoal()) << std::endl;
+            return false;
+        }
+
+        if (!executor_client_->start_plan_execution(plan.value())) 
+        {
+            RCLCPP_ERROR(get_logger(), "Error starting a new plan (first)");
+        }
+
+        return true;
+    }
+
+    void init_knowledge()
+    {
+        problem_expert_->addInstance(plansys2::Instance{"rasms", "robot"});
+        problem_expert_->setGoal(plansys2::Goal("robot moved"));
+    }
+
+    void step()
+    {
+        if (!executor_client_->execute_and_check_plan()) 
+        {  // Plan finished
+            auto result = executor_client_->getResult();
+
+            if (result.value().success) 
+            {
+                RCLCPP_INFO(get_logger(), "Plan succesfully finished");
+            } else {
+                RCLCPP_ERROR(get_logger(), "Plan finished with error");
+            }
+        }
+    }
+
+private:
+    std::shared_ptr<plansys2::DomainExpertClient> domain_expert_;
+    std::shared_ptr<plansys2::PlannerClient> planner_client_;
+    std::shared_ptr<plansys2::ProblemExpertClient> problem_expert_;
+    std::shared_ptr<plansys2::ExecutorClient> executor_client_;
+};
+
+int main(int argc, char ** argv)
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<Assemble>();
+
+    if (!node->init()) 
+    {
+        return 0;
+    }
+
+    rclcpp::Rate rate(5);
+    while (rclcpp::ok()) 
+    {
+        node->step();
+
+        rate.sleep();
+        rclcpp::spin_some(node->get_node_base_interface());
+    }
+
+    rclcpp::shutdown();
+
+    return 0;
+}

rasms_manipulator/src/behavior_tree_nodes/Move.cpp

@@ -3,113 +3,133 @@
 
 #include "rasms_bt/behavior_tree_nodes/Move.hpp"
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/moveit_cpp/planning_component.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include <geometry_msgs/msg/point_stamped.h>
+#include <geometry_msgs/msg/pose.h>
+
+#include <behaviortree_cpp_v3/behavior_tree.h>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
 
 namespace rasms_manipulation
 {
 Move::Move(
-    const std::string& xml_tag_name, 
+    const std::string& xml_tag_name,
+    const std::string& action_name, 
     const BT::NodeConfiguration& conf)
-    : BT::ActionNodeBase(xml_tag_name, conf)
+    : plansys::BtActionNode<moveit_msgs::MoveGroupAction>(xml_tag_name, action_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);
+    rclcpp::Node::SharedPtr node;
+    config().blackboard->get("node", 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;
+        node->declare_parameter("waypoints");
+        node->declare_parameter("waypoint_coords");
+    }
+    catch(const rclcpp::exceptions::ParameterAlreadyDeclaredException& ex) {
+        std::cerr << ex.what() << '\n';
+    }
+    
+    if (node->has_parameter("waypoints"))
+    {
+        std::vector<std::string> wp_names;
+        node->get_parameter_or("waypoints", wp_names, {});
+
+        for(auto& wp: wp_names)
+        {
+            try
+            {
+                node->declare_parameter("waypoint_coords." + wp);
+            } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException& ex) {
+                std::cerr << ex.what() << '\n';
+            }
+
+            std::vector<double> coords;
+            if (node->get_parameter_or("waypoint_coords." + wp, coords, {}))
+            {
+                geometry_msgs::msg::Pose pose;
+                pose.position.x = coords[0];
+                pose.position.y = coords[1];
+                pose.position.z = coords[2];
+                pose.position.w = coords[3]
+                // pose.orientation.x = coords[3];
+                // pose.orientation.y = coords[4];
+                // pose.orientation.z = coords[5];
+                // pose.orientation.w = coords[6];
+
+                waypoints_[wp] = pose;
+            } else {
+                std::cerr << "No coordinate configured for waypoint [" << wp << "]" << std::endl;
+            }
+        }
     }
 }
 
-BT::NodeStatus Move::on_tick()
+void 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::NodeOptions node_options;
+    node_options.automatically_declare_parameters_from_overrides(true);
+    rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("moveit_cpp_move", node_options);
+    config().blackboard->get("node", node);
+
+
+    std::string goal;
+    getInput<std::string>("goal", goal);
+
+    geometry_msgs::msg::Pose pose2moveit;
+    if (waypoints_.find(goal) != waypoints_.end()) {
+        pose2moveit = waypoints_[goal];
+    } else {
+        std::cerr << "No coordinate for waypoint [" << goal << "]" << std::endl;
     }
 
-    rclcpp:spin_some(node_);
+    std::string PLANNING_GROUP = "rasms_arm_group";\
 
-    if (result_ == 0)
+    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
+    geometry_msgs::msg::PoseStamped goal_pos;
+
+    goal_pos.header.frame_id = "base";
+    goal_pos.pose.position.x = pose2moveit.position.x;
+    goal_pos.pose.position.y = pose2moveit.position.y;
+    goal_pos.pose.position.z = pose2moveit.position.z;
+    goal_pos.pose.position.w = pose2moveit.position.w
+    // goal_pos.pose.orientation = pose2moveit.orientation;
+    
+    move_group.setPoseTarget(goal_pose, "link6");
+    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
+    moveit::planning_interface::MoveItErrorCode success = move_group(my_plan);
+    if (success)
     {
-        return BT::NodeStatus::RUNNING;
-    } else if (result_ == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
-    {
-        result_ = 0;
-        RCLCPP_INFO(node_->get_logger(), "Success!");
-        return BT::NodeStatus::SUCCESS;
+        move_group.execute(my_plan);
     } else {
-        RCLCPP_ERROR(node_->get_logger(), "Move error: MoveItErrorCodes[%i]", result_);
-        return BT::NodeStatus::FAILURE;
+        return;
     }
 }
-} //namespace rasms_manipulator
 
+BT::NodeStatus
+Move::on_success()
+{
+  return BT::NodeStatus::SUCCESS;
+}
+
+
+}  // namespace plansys2_bt_tests
+
+#include "behaviortree_cpp_v3/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
-    factory.registerNodeType<rasms_manipulation::Move>("Move");
+  BT::NodeBuilder builder =
+    [](const std::string & name, const BT::NodeConfiguration & config)
+    {
+      return std::make_unique<rasms_manipulation::Move>(
+        name, "navigate_to_pose", config);
+    };
+
+  factory.registerBuilder<rasms_manipulation::Move>(
+    "Move", builder);
 }