rebuild move logic

rasms_manipulator/.vscode/c_cpp_properties.json

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

rasms_manipulator/.vscode/settings.json

@@ -0,0 +1,9 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/foxy/lib/python3.8/site-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/foxy/lib/python3.8/site-packages"
+    ],
+    "ros.distro": "foxy"
+}

rasms_manipulator/CMakeLists.txt

@@ -26,8 +26,7 @@ endif()
 set(dependencies
     rclcpp
     rclcpp_action
-    geometry_msgs
-    tf2_geometry_msgs
+    geometry_msgs    tf2_geometry_msgs
     moveit_msgs
     moveit_core
     moveit_ros_planning
@@ -52,13 +51,17 @@ foreach(bt_plugin ${plugin_libs})
     target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
 endforeach()
 
-add_executable(assemble_controller_node src/assemble_controller_node.cpp)
-ament_target_dependencies(assemble_controller_node ${dependencies})
+add_executable(rasms_controller_node src/rasms_controller_node.cpp)
+ament_target_dependencies(rasms_controller_node ${dependencies})
+
+add_executable(rasms_action_node src/rasms_action_node.cpp)
+ament_target_dependencies(rasms_action_node ${dependencies})
 
 install(DIRECTORY launch pddl behavior_trees_xml config DESTINATION share/${PROJECT_NAME})
 
 install(TARGETS
-    assemble_controller_node
+    rasms_controller_node
+    rasms_action_node
     ${plugin_libs}
     ARCHIVE DESTINATION lib
     LIBRARY DESTINATION lib

rasms_manipulator/config/params.yaml

@@ -1,7 +1,8 @@
-move_1:
+move:
   ros__parameters:
     plugins:
       - rasms_move_bt_node
-    waypoints: ["one"]
+    waypoints: ["one", "two"]
     waypoints_coords:
-      one: [0.2, 0.2, 0.2]
+      one: [0.0, 0.0, 0.0]
+      two: [0.2, 0.2, 0.2]

rasms_manipulator/launch/rasms_manipulation.launch.py

@@ -35,7 +35,7 @@ def generate_launch_description():
     move_1 = Node(
         package='plansys2_bt_actions',
         executable='bt_action_node',
-        name='move_1',
+        name='move',
         namespace=namespace,
         output='screen',
         parameters=[

rasms_manipulator/pddl/domain.pddl

@@ -22,13 +22,14 @@
 
 ;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 (:durative-action move
-    :parameters (?r - robot ?z - zone)
+    :parameters (?r - robot ?z1 ?z2 - zone)
     :duration ( = ?duration 5)
     :condition (and
-        (over all(robot_at ?r ?z))
+        (at start(robot_at ?r ?z1))
     )
     :effect (and
-        (at end(robot_moved ?r ?z))
+        (at start(not(robot_at ?r ?z1)))
+        (at end(robot_at ?r ?z2))
     )
 )
 

rasms_manipulator/src/behavior_tree_nodes/Move.cpp

@@ -32,15 +32,8 @@ namespace rasms_manipulation
             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, {}))
@@ -77,35 +70,36 @@ namespace rasms_manipulation
             std::cerr << "No coordinate for waypoint [" << goal << "]" << std::endl;
         }
 
+        //Setup MoveIt interface for planning
         static const std::string PLANNING_GROUP = "rasms_arm_group";
-        moveit::planning_interface::MoveGroupInterface move_group(node, PLANNING_GROUP);
-        // move_group->setStartState(*move_group->getCurrentState());
+        moveit::planning_interface::MoveGroupInterface move_group_interface(node, PLANNING_GROUP);
+        
+        // Raw pointers are frequently used to refer to the planning group for improved performance.
+        const moveit::core::JointModelGroup* joint_model_group =
+            move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
+        
+        // move_group_interface->setStartState(*move_group_interface->getCurrentState());
 
         /*----------
   00823   // pose goal;
   00824   // for each link we have a set of possible goal locations;
   00825   std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
   ----------*/
-        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.orientation = pose2moveit.orientation;
+        geometry_msgs::msg::Pose goal_pos;
+        goal_pos.orientation.w = 1.0;
+        goal_pos.position.x = pose2moveit.position.x;
+        goal_pos.position.y = pose2moveit.position.y;
+        goal_pos.position.z = pose2moveit.position.z;
+        move_group_interface.setPoseTarget(goal_pos);
 
-        move_group.setPoseTarget(goal_pos, "link6");
         moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-        bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+        bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
         if (success)
         {
             //RCLCPP_INFO("Planning successful");
-            // move_group->setStartStateToCurrentState();
-            move_group.execute(my_plan);
-            move_group.move();
-        }
-        else
-        {
-            return;
+            // move_group_interface->setStartStateToCurrentState();
+            move_group_interface.execute(my_plan);
+            move_group_interface.move();
         }
     }
 

rasms_manipulator/src/rasms_action_node.cpp

@@ -26,7 +26,7 @@ class AssembleAction : public plansys2::ActionExecutorClient
 {
 public:
   AssembleAction()
-  : plansys2::ActionExecutorClient("assemble", 500ms)
+  : plansys2::ActionExecutorClient("rasms_move", 500ms)
   {
     progress_ = 0.0;
   }
@@ -36,16 +36,16 @@ private:
   {
     if (progress_ < 1.0) {
       progress_ += 0.05;
-      send_feedback(progress_, "Assemble running");
+      send_feedback(progress_, "Move running");
     } else {
-      finish(true, 1.0, "Assemble completed");
+      finish(true, 1.0, "Move 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::cout << "Moving ... [" << std::min(100.0, progress_ * 100.0) << "%]  " <<
       std::flush;
   }
 
@@ -57,7 +57,7 @@ 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->set_parameter(rclcpp::Parameter("action_name", "move"));
   node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
 
   rclcpp::spin(node->get_node_base_interface());

rasms_manipulator/src/rasms_controller_node.cpp

@@ -1,3 +1,17 @@
+// 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 <random>
 
@@ -11,11 +25,13 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 
-class RasmsController: public rclcpp::Node
+class RasmsController : public rclcpp::Node
 {
 public:
-    RasmsController(): rclcpp::Node("rasms_controller")
-    {}
+    RasmsController()
+        : rclcpp::Node("rasms_controller")
+    {
+    }
 
     void init()
     {
@@ -36,17 +52,19 @@ public:
             {
                 RCLCPP_ERROR(get_logger(), "Error starting a new plan (first)");
             }
-        } else {
+        }
+        else
+        {
             RCLCPP_ERROR_STREAM(
-                this->get_logger(), "Could not find plan to reach goal " <<
-                parser::pddl::toString(problem_expert_->getGoal()));
+                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"));
+        problem_expert_->addInstance(plansys2::Instance{"one", "zone"});
+        problem_expert_->addInstance(plansys2::Instance{"two", "zone"});
     }
 
     std::string status_to_string(int8_t status)
@@ -89,7 +107,7 @@ private:
     std::shared_ptr<plansys2::PlannerClient> planner_client_;
 };
 
-int main(int argc, char** argv)
+int main(int argc, char **argv)
 {
     rclcpp::init(argc, argv);
     auto node = std::make_shared<RasmsController>();

rasms_manipulator/src/rasmsnode.cpp

@@ -1,17 +1,3 @@
-// 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 <random>
 
@@ -25,13 +11,11 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 
-class DummyController : public rclcpp::Node
+class RasmsController: public rclcpp::Node
 {
 public:
-    DummyController()
-        : rclcpp::Node("dummy_controller")
-    {
-    }
+    RasmsController(): rclcpp::Node("rasms_controller")
+    {}
 
     void init()
     {
@@ -52,19 +36,17 @@ public:
             {
                 RCLCPP_ERROR(get_logger(), "Error starting a new plan (first)");
             }
-        }
-        else
-        {
+        } else {
             RCLCPP_ERROR_STREAM(
-                this->get_logger(), "Could not find plan to reach goal " << parser::pddl::toString(problem_expert_->getGoal()));
+                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_->addInstance(plansys2::Instance{"zone", "zone"});
-        problem_expert_->setGoal(plansys2::Goal("(and(robot_moved rasms zone))"));
+        problem_expert_->setGoal(plansys2::Goal("robot moved"));
     }
 
     std::string status_to_string(int8_t status) 
@@ -107,10 +89,10 @@ private:
     std::shared_ptr<plansys2::PlannerClient> planner_client_;
 };
 
-int main(int argc, char **argv)
+int main(int argc, char** argv)
 {
     rclcpp::init(argc, argv);
-    auto node = std::make_shared<DummyController>();
+    auto node = std::make_shared<RasmsController>();
 
     node->init();