write move action with moveit

rasms_manipulator/config/params.yaml

@@ -5,4 +5,4 @@ move:
     waypoints: ["one", "two"]
     waypoint_coords:
       one: [0.0, 0.0, 0.0]
-      two: [0.28, -0.2, 0.5]
+      two: [0.01, 0.01, 0.6]

rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp

@@ -13,7 +13,7 @@
 
 namespace rasms_manipulation
 {
-    class Move : public BT::AsyncActionNode
+    class Move : public BT::ActionNodeBase
     {
     public:
         Move(const std::string &xml_tag_name, 
@@ -32,6 +32,7 @@ namespace rasms_manipulation
     private:
         std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
         std::atomic_bool _halt_requested;
+        int counter_;
     };
 
 } // namespace rasms_manipulation

rasms_manipulator/launch/rasms_manipulation.launch.py

@@ -1,16 +1,55 @@
 import os
+import yaml
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
+def load_file(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, "r") as file:
+            return file.read()
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, "r") as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
 
 def generate_launch_description():
 
+     # planning_context
+    robot_description_config = load_file(
+        "rasms_description", "urdf/rasms_description.urdf.xacro"
+    )
+    robot_description = {"robot_description": robot_description_config}
+
+    robot_description_semantic_config = load_file(
+        "rasms_moveit_config", "srdf/rasms_description.srdf"
+    )
+    robot_description_semantic = {
+        "robot_description_semantic": robot_description_semantic_config
+    }
+
+    kinematics_yaml = load_yaml(
+        "rasms_moveit_config", "config/kinematics.yml"
+    )
+
     pkg_dir = get_package_share_directory('rasms_manipulator')
     namespace = LaunchConfiguration('namespace')
 
@@ -40,16 +79,21 @@ def generate_launch_description():
         output='screen',
         parameters=[
           pkg_dir + '/config/params.yaml',
+          robot_description, 
+          robot_description_semantic, 
+          kinematics_yaml,
           {
             'action_name': 'move',
-            ### [Groot](https://github.com/BehaviorTree/Groot) behavior_tree_visual tool
-            ### [](https://intelligentroboticslab.gsyc.urjc.es/ros2_planning_system.github.io/design/index.html)
-            # need to provide visual information about working behavior tree
-            # 'publisher_port': 1668,
-            # 'server_port': 1669,
             'bt_xml_file': pkg_dir + '/behavior_trees_xml/move.xml'
           }
         ])
+    transport_1_cmd = Node(
+        package='rasms_manipulator',
+        executable='rasms_action_node',
+        name='transport_1',
+        namespace=namespace,
+        output='screen',
+        parameters=[])   # Create the launch description and populate
     
     ld = LaunchDescription()
 
@@ -59,5 +103,6 @@ def generate_launch_description():
     # Declare the launch options
     ld.add_action(plansys2_cmd)
     ld.add_action(move_1)
+    # ld.add_action(transport_1_cmd)
   
     return ld

rasms_manipulator/pddl/commands

@@ -1,6 +1,6 @@
 set instance rasms robot
 set instance one zone
+set instance two zone
 set predicate (robot_at rasms one)
-set goal (and(robot_moved rasms one))
+set goal (and(robot_at rasms two))
 run
-

rasms_manipulator/src/behavior_tree_nodes/Move.cpp

@@ -14,9 +14,11 @@
 
 namespace rasms_manipulation
 {
+    static const rclcpp::Logger LOGGER = rclcpp::get_logger("plansys2_move_node");
+
     Move::Move(const std::string &xml_tag_name, 
                 const BT::NodeConfiguration &conf)
-        : AsyncActionNode(xml_tag_name, conf)
+        : ActionNodeBase(xml_tag_name, conf), counter_(0)
     {
         rclcpp::Node::SharedPtr node;
         config().blackboard->get("node", node);
@@ -28,7 +30,7 @@ namespace rasms_manipulation
         }
         catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &ex)
         {
-            std::cerr << ex.what() << '\n';
+            RCLCPP_WARN(LOGGER, "%c", ex.what());
         }
 
         if (node->has_parameter("waypoints"))
@@ -56,7 +58,7 @@ namespace rasms_manipulation
                 }
                 else
                 {
-                    std::cerr << "No coordinate configured for waypoint [" << wp << "]" << std::endl;
+                    RCLCPP_WARN(LOGGER, "No coordinate configured for waypoint [%s]", wp.c_str());
                 }
             }
         }
@@ -65,17 +67,21 @@ namespace rasms_manipulation
     void Move::halt()
     {
         _halt_requested = true;
-        std::cout << "Move halt" << std::endl;
+        RCLCPP_INFO(LOGGER, "Move halt");
     }
 
     BT::NodeStatus  Move::tick()
     {
-        rclcpp::Node::SharedPtr node;
-        config().blackboard->get("node", node);
 
-        // rclcpp::executors::SingleThreadedExecutor executor;
+        rclcpp::NodeOptions node_options;
-        // executor.add_node(node);
+        node_options.automatically_declare_parameters_from_overrides(true);
-        // std::thread([&executor]() {executor.spin();}).detach();
+        auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);
+
+        config().blackboard->get("node", move_group_node);
+
+        rclcpp::executors::SingleThreadedExecutor executor;
+        executor.add_node(move_group_node);
+        std::thread([&executor]() { executor.spin(); }).detach();
 
         std::string goal;
         getInput<std::string>("goal", goal);
@@ -84,53 +90,46 @@ namespace rasms_manipulation
         if (waypoints_.find(goal) != waypoints_.end())
         {
             pose2moveit = waypoints_[goal];
-            std::cout << pose2moveit.position.x << ", " << pose2moveit.position.y << ", " << pose2moveit.position.z << std::endl;
+            RCLCPP_INFO(LOGGER, "Read goal [%f, %f, %f]", pose2moveit.position.x, pose2moveit.position.y, pose2moveit.position.z);
+        } else {
+            RCLCPP_WARN(LOGGER, "No coordinate for waypoint [%s]", goal.c_str());
         }
-        else
-        {
-            std::cerr << "No coordinate for waypoint [" << goal << "]" << std::endl;
-        }
-        
         //Setup MoveIt interface for planning
-        moveit::planning_interface::MoveGroupInterface::Options move_group_options("rasms_arm_group", "/robot_description");
+        static const std::string PLANNING_GROUP = "rasms_arm_group";
-        moveit::planning_interface::MoveGroupInterface move_group_interface(node, move_group_options);
+        RCLCPP_INFO(LOGGER, "MoveGroupInterface set current planning group `%s`", PLANNING_GROUP.c_str());
 
-        // Raw pointers are frequently used to refer to the planning group for improved performance.
+        moveit::planning_interface::MoveGroupInterface::Options move_group_options(PLANNING_GROUP, "robot_description");
-        // const moveit::core::JointModelGroup* joint_model_group =
+        moveit::planning_interface::MoveGroupInterface move_group_interface(move_group_node, move_group_options);
-        //     move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
             
-        // move_group_interface->setStartState(*move_group_interface->getCurrentState());
+        moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
+        move_group_interface.setStartState(start_state);
 
-        /*----------
-  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::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;
-        std::cout << "[MOVE_STARTED]: goal [" << 
+
-            goal_pos.position.x << ", " << 
+        RCLCPP_INFO(LOGGER, "Move started to goal [%f, %f, %f]", goal_pos.position.x, goal_pos.position.y, goal_pos.position.z);
-            goal_pos.position.y << ", " <<
-            goal_pos.position.z << ", " <<
-            "]" << std::endl;
         move_group_interface.setPoseTarget(goal_pos);
 
         moveit::planning_interface::MoveGroupInterface::Plan my_plan;
         bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
         if (success)
         {
-            //RCLCPP_INFO("Planning successful");
+            RCLCPP_INFO(LOGGER, "Planning success");
-            // move_group_interface.setStartStateToCurrentState();
             move_group_interface.execute(my_plan);
             move_group_interface.move();
+            counter_++;
         } else {
-            std::cout << " failed to generate a plan" << std::endl;
+
+            RCLCPP_WARN(LOGGER, "Failed to generate a plan");
             return BT::NodeStatus::FAILURE; 
         }
 
+        if (counter_ != waypoints_.size())
+            return BT::NodeStatus::RUNNING;
+            
         return BT::NodeStatus::SUCCESS; 
     }
 

rasms_manipulator/src/rasms_controller_node.cpp

@@ -12,15 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <plansys2_pddl_parser/Utils.h>
+
 #include <memory>
-#include <random>
 
 #include "plansys2_msgs/msg/action_execution_info.hpp"
+#include "plansys2_msgs/msg/plan.hpp"
 
-#include "plansys2_executor/ExecutorClient.hpp"
 #include "plansys2_domain_expert/DomainExpertClient.hpp"
-#include "plansys2_problem_expert/ProblemExpertClient.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"
@@ -36,9 +38,9 @@ public:
     void 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>();
-        planner_client_ = std::make_shared<plansys2::PlannerClient>();
 
         init_knowledge();
 

rasms_moveit_config/launch/rasms_control.launch.py

@@ -86,7 +86,7 @@ def generate_launch_description():
         arguments=["rasms_arm_controller", "--controller-manager", "/controller_manager"],
     )
 
-    """load_controllers = []
+    load_controllers = []
     for controller in [
         "rasms_arm_controller",
         "joint_state_broadcaster",
@@ -97,17 +97,17 @@ def generate_launch_description():
                 shell=True,
                 output="screen",
             )
-        ]"""
+        ]
 
     return LaunchDescription(
         launch_args + 
-        #load_controllers +
+        load_controllers +
         [
             controller_manager,
             robot_state_publisher,
-            joint_state_publisher,
             static_tf,
             joint_state_broadcaster,
-            rasms_arm_controller
+            rasms_arm_controller,
+            joint_state_publisher
         ] 
         )