Add visual meshes for rasmt_support package and small fixes

rasmt_moveit_config/config/rasmt_moveit.rviz

@@ -326,18 +326,18 @@ Window Geometry:
     collapsed: false
   Height: 1016
   Hide Left Dock: false
-  Hide Right Dock: false
+  Hide Right Dock: true
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000210000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000253000001880000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000042a0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000210000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000253000001880000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000053f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
+    collapsed: true
   Width: 1848
-  X: 315
-  Y: 209
+  X: 520
+  Y: 200

rasmt_support/urdf/rasmt.xacro

@@ -8,7 +8,7 @@
 <link name="world"/>
 
 <xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_macro.xacro"/>
-<xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)"/>
+<xacro:rasmt_single prefix="$(arg robot_name)" parent="world" sim="$(arg sim)" xyz="0 0 0"/>
 
 <xacro:if value="$(arg grip)">
     <xacro:include filename="$(find rasmt_support)/urdf/tools/rasmt_hand_macro.xacro"/>

rasmt_support/urdf/robot/rasmt_single_macro.xacro

@@ -4,14 +4,14 @@
   <xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_control.xacro"/>
   <xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_gazebo.xacro"/>
 
-<xacro:macro name="rasmt_single" params="prefix parent sim:=^|true">
+<xacro:macro name="rasmt_single" params="prefix parent sim xyz">
 
 
   <joint name="to_${parent}" type="fixed">
     <parent link="${parent}"/>
     <child link="${prefix}_Base_Link"/>
     <!--origin xyz="0 0 1.15" rpy="3.14159 0 3.14159"/-->
-    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <origin xyz="${xyz}" rpy="0 0 0"/>
     <axis xyz="0 0 0"/>
   </joint>
 
@@ -31,7 +31,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="package://rasmt_support/meshes/collision/Base_Link.STL" />
+        <mesh filename="package://rasmt_support/meshes/visual/Base_Link.dae" />
       </geometry>
       <material
         name="">
@@ -61,7 +61,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="package://rasmt_support/meshes/collision/Fork_1.STL" />
+        <mesh filename="package://rasmt_support/meshes/visual/Fork_1.dae" />
       </geometry>
       <material
         name="">
@@ -121,7 +121,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Link_1.STL" />
+          filename="package://rasmt_support/meshes/visual/Link_1.dae" />
       </geometry>
       <material
         name="">
@@ -181,7 +181,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Fork_2.STL" />
+          filename="package://rasmt_support/meshes/visual/Fork_2.dae" />
       </geometry>
       <material
         name="">
@@ -241,7 +241,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Link_2.STL" />
+          filename="package://rasmt_support/meshes/visual/Link_2.dae" />
       </geometry>
       <material
         name="">
@@ -301,7 +301,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Fork_3.STL" />
+          filename="package://rasmt_support/meshes/visual/Fork_3.dae" />
       </geometry>
       <material
         name="">
@@ -361,7 +361,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Dock_Link.STL" />
+          filename="package://rasmt_support/meshes/visual/Dock_Link.dae" />
       </geometry>
       <material
         name="">

rasmt_support/urdf/tools/rasmt_hand_macro.xacro

@@ -28,7 +28,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Grip_Body.STL" />
+          filename="package://rasmt_support/meshes/visual/Grip_Body.dae" />
       </geometry>
       <material
         name="">
@@ -83,7 +83,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Grip_L.STL" />
+          filename="package://rasmt_support/meshes/visual/Grip_L.dae" />
       </geometry>
       <material
         name="">
@@ -143,7 +143,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Grip_R.STL" />
+          filename="package://rasmt_support/meshes/visual/Grip_R.dae" />
       </geometry>
       <material
         name="">

robossembler/behavior_trees_xml/atomic_skills_xml/grasp_part.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="GraspPart">
+    <!-- ////////// -->
+    <BehaviorTree ID="GraspPart">
+        <Fallback name="root_Fallback">
+            <Sequence name="GraspSkill">
+                <ReactiveFallback name="PartEstimate">
+                    <Condition ID="PartInScene"/>
+                    <Action ID="FindPart" PartName="{arg1}" PartPose="{arg0}"/>
+                </ReactiveFallback>
+                <Fallback name="Move">
+                    <Condition ID="PathFree"/>
+                    <Action ID="MoveToPoint" arm_group="${arg2}" goal="${arg0}"/>
+                </Fallback>
+                <Action ID="GraspPart" arm_group="${arg3}"/>
+                <Action ID="MoveToPoint" arm_group="{arg2}" goal="{arg4}"/>
+            </Sequence>
+        </Fallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="FindPart">
+            <inout_port name="PartName"/>
+            <output_port name="PartPose"/>
+        </Action>
+        <Action ID="GraspPart">
+            <input_port name="arm_group"/>
+        </Action>
+        <Action ID="MoveToPoint">
+            <input_port name="arm_group"/>
+            <input_port name="goal"/>
+        </Action>
+        <Condition ID="PartInScene"/>
+        <Condition ID="PathFree"/>
+        <Action ID="SpawnPart">
+            <inout_port name="PartName"/>
+            <input_port name="PartPose"/>
+        </Action>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
+

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

@@ -8,7 +8,7 @@
 
 #include "geometry_msgs/msg/pose.hpp"
 #include "moveit_msgs/msg/move_it_error_codes.hpp"
-#include "plansys2_bt_actions/BTActionNode.hpp"
+//#include "plansys2_bt_actions/BTActionNode.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 namespace task_planner

robossembler/pddl/domain.pddl

@@ -26,4 +26,4 @@
     :effect (and (at end(robot_move ?r ?z)))
 )
 
-);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;
+);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;

robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp

@@ -2,7 +2,7 @@
 #include <thread>
 
 #include "rclcpp/executors/single_threaded_executor.hpp"
-#include "rclcpp_lifecycle/lifecycle_node.hpp"
+//#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp/qos.hpp"
 
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"