18: rename package & update description

README.md

@@ -2,7 +2,19 @@
 
 Repo for ROS2 packages related to Robossembler
 
-### Install
+## Instructions
+### Requirements
+* OS: Ubuntu 20.04
+* Other distributions might work (not tested).
+
+### Dependencies
+These are the primary dependencies required to use this project.
+
+* ROS 2 Foxy
+* MoveIt 2 
+    > Install/build a version based on the selected ROS 2 release
+
+### Build
 
 1. Clone the repository
 2. Build packages
@@ -17,7 +29,7 @@ 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
+git clone https://gitlab.com/robosphere/robossembler-ros2
 vcs import . < robossembler-ros2/rasms.repos
 cd ..
 rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy
@@ -26,7 +38,7 @@ colcon build --symlink-install --mixin release
 
 but at the moment the visualization is not implemented
 
-### Run
+### Examples
 Add source to environment
 ```
 source install/setup.bash
@@ -37,7 +49,7 @@ 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
+ros2 launch robossembler robossembler.launch.py
 ```
 Launch Plansys2 Terminal
 ```bash
@@ -45,11 +57,9 @@ ros2 run plansys2_terminal plansys2_terminal
 ```
 Then into plansys2_terminal paste command (see updates into pddl/commands)
 ```bash
-set instance rasms robot
+set instance rasms_arm_group robot
 set instance one zone
-set instance two zone
-set predicate (robot_at rasms one)
-set goal (and(robot_at rasms two))
+set goal (and (robot_move rasms_arm_group one))
 run
 ```
 

task_planner/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(task_planner)
+project(robossembler)
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -45,8 +45,8 @@ set(dependencies
 
 include_directories(include)
 
-add_library(task_planner_move_bt_node SHARED src/behavior_tree_nodes/atomic_skills/Move.cpp)
-list(APPEND plugin_libs task_planner_move_bt_node)
+add_library(robossembler_move_bt_node SHARED src/behavior_tree_nodes/atomic_skills/Move.cpp)
+list(APPEND plugin_libs robossembler_move_bt_node)
 
 foreach(bt_plugin ${plugin_libs})
     ament_target_dependencies(${bt_plugin} ${dependencies})

task_planner/README.md

@@ -13,7 +13,7 @@ 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
+git clone https://gitlab.com/robosphere/robossembler-ros2
 vcs import . < robossembler-ros2/rasms.repos
 cd ..
 rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy

task_planner/config/params.yaml

@@ -1,8 +1,10 @@
 move:
   ros__parameters:
     plugins:
-      - task_planner_move_bt_node
+      - robossembler_move_bt_node
     arm_group: ["rasms_arm_group"]
     waypoints: ["one"]
     waypoint_coords:
-      one: [0.01, 0.01, 0.6]
+      one: [0.01, 0.01, 0.6,
+            0.0 , 0.0 , 0.0, 
+            1.0]

task_planner/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp

@@ -4,6 +4,7 @@
 
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
+
 #include "geometry_msgs/msg/pose.hpp"
 #include "moveit_msgs/msg/move_it_error_codes.hpp"
 #include "plansys2_bt_actions/BTActionNode.hpp"
@@ -19,8 +20,8 @@ namespace task_planner
 
         void resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg);
 
-        virtual void halt() override;
-        BT::NodeStatus tick() override;
+        virtual void halt();
+        BT::NodeStatus tick();
 
         static BT::PortsList providedPorts()
         {

task_planner/launch/task_planner.launch.py

@@ -85,7 +85,6 @@ def generate_launch_description():
           {
             'action_name': 'move',
             'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/move.xml',
-            'print_graph': True
           }
         ])
 
@@ -104,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)
+    
     return ld

task_planner/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xds" schematypens="http://www.w3.org/2001/XMLSchema"?>
 
 <package format="3">
- <name>task_planner</name>
+ <name>robossembler</name>
  <version>0.0.1</version>
 
  <description>ROS2 task planner for manipulator</description>

task_planner/pddl/domain.pddl

@@ -1,4 +1,4 @@
-(define (domain rasms)
+(define (domain robossembler)
 (:requirements :strips :typing :adl :fluents :durative-actions :typing)
 
 ;; Types ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

task_planner/src/behavior_tree_nodes/atomic_skills/Move.cpp

@@ -1,13 +1,10 @@
 #include <iostream>
-#include <string>
 #include <thread>
 
 #include "rclcpp/executors/single_threaded_executor.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
-#include "rclcpp/rclcpp.hpp"
 #include "rclcpp/qos.hpp"
 
-#include "geometry_msgs/msg/pose.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 
 #include "moveit/move_group_interface/move_group_interface.h"
@@ -65,6 +62,18 @@ namespace task_planner
                     pose.position.x = coords[0];
                     pose.position.y = coords[1];
                     pose.position.z = coords[2];
+                    if (coords.size() < 4)
+                    {
+                        pose.orientation.x = 0.0;
+                        pose.orientation.y = 0.0;
+                        pose.orientation.z = 0.0;
+                        pose.orientation.w = 1.0;
+                    } else {
+                        pose.orientation.x = coords[3];
+                        pose.orientation.y = coords[4];
+                        pose.orientation.z = coords[5];
+                        pose.orientation.w = coords[6];
+                    }
                     waypoints_[wp] = pose;
                 }
                 else
@@ -116,7 +125,11 @@ namespace task_planner
         move_group_interface.setStartState(start_state);
 
         geometry_msgs::msg::Pose goal_pos;
-        goal_pos.orientation.w = 1.0;
+        goal_pos.orientation.x = pose2moveit.orientation.x;
+        goal_pos.orientation.y = pose2moveit.orientation.y;
+        goal_pos.orientation.z = pose2moveit.orientation.z;
+        goal_pos.orientation.w = pose2moveit.orientation.w;
+
         goal_pos.position.x = pose2moveit.position.x;
         goal_pos.position.y = pose2moveit.position.y;
         goal_pos.position.z = pose2moveit.position.z;

task_planner/src/move_action_node.cpp

@@ -20,13 +20,15 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 
+#include "robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp"
+
 using namespace std::chrono_literals;
 
 class MoveAction : public plansys2::ActionExecutorClient
 {
 public:
   MoveAction()
-  : plansys2::ActionExecutorClient("atomic_skill_move", 500ms)
+  : plansys2::ActionExecutorClient("move", 500ms)
   {
     progress_ = 0.0;
   }