minor adjustments all over the place.

src/btree/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
-project(btree_nodes)
+project(btree)
 
 add_compile_options(-Wall -Wextra -Wpedantic)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
@@ -36,10 +36,8 @@ endforeach()
 
 set(CPP_FILES
         src/Tree.cpp
-        src/Factory.cpp
         src/Extensions.cpp
         src/nodes/WeightedRandomNode.cpp
-        src/nodes/AmICalled.cpp
         src/nodes/GenerateXYPose.cpp
         src/nodes/RobotMove.cpp
         src/nodes/MoveConnection.cpp
@@ -52,31 +50,16 @@ set(CPP_FILES
         src/nodes/ActorMovement.cpp
         )
 
-add_library(tree_plugins_base src/Factory.cpp)
-set_property(TARGET tree_plugins_base PROPERTY CXX_STANDARD 17)
 add_executable(tree ${CPP_FILES})
 set_property(TARGET tree PROPERTY CXX_STANDARD 17)
-ament_target_dependencies(tree_plugins_base ${DEPENDENCIES})
 ament_target_dependencies(tree ${DEPENDENCIES})
 
-pluginlib_export_plugin_description_file(behaviortree_cpp_v3 plugins.xml)
-
-#add_executable(talker src/publisher_member_function.cpp)
-#ament_target_dependencies(talker geometry_msgs rclcpp)
-#add_executable(listener src/subscriber_member_function.cpp)
-#ament_target_dependencies(listener geometry_msgs rclcpp)
-
-install(TARGETS
-        tree_plugins_base
-        EXPORT export_${PROJECT_NAME}
-        ARCHIVE DESTINATION lib
-        LIBRARY DESTINATION lib
-        RUNTIME DESTINATION bin)
-
 install(TARGETS
         tree
         DESTINATION lib/${PROJECT_NAME})
         
+install(DIRECTORY   trees     DESTINATION   share/${PROJECT_NAME})
+
 if (BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
     # the following line skips the linter which checks for copyrights
@@ -88,11 +71,4 @@ if (BUILD_TESTING)
     ament_lint_auto_find_test_dependencies()
 endif ()
 
-ament_export_libraries(
-        tree_plugins_base
-)
-ament_export_targets(
-        export_${PROJECT_NAME}
-)
-
 ament_package()

src/btree/nodes.xml

@@ -70,7 +70,7 @@
         </Action>
 
         <Action ID="RandomFailure">
-            <input_port name="failureChance">Chance to fail from 0 to 1</input_port>
+            <input_port name="failure_chance">Chance to fail from 0 to 1</input_port>
         </Action>
     </TreeNodesModel>
 </root>

src/btree/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>btree_nodes</name>
+  <name>btree</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="bastian@todo.todo">bastian</maintainer>

src/btree/src/Extensions.cpp

@@ -38,15 +38,22 @@ namespace BT {
     template<>
     std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
         auto parts = splitString(str, ',');
-        if (parts.size() != 3) {
-            throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
+        if (!(parts.size() == 3 || parts.size() == 7)) {
+            throw RuntimeError("Incorrect number of arguments, expected 3 or 7 in format '<x>,<y>,<z>[,<qw>,<qx>,<qy>,<qz>]'.");
         }
 
         auto pose = std::make_shared<geometry_msgs::msg::Pose>();
+        if(parts.size() == 7){
+            pose->orientation.w = convertFromString<double>(parts[3]);
+            pose->orientation.x = convertFromString<double>(parts[4]);
+            pose->orientation.y = convertFromString<double>(parts[5]);
+            pose->orientation.z = convertFromString<double>(parts[6]);
+        }else{
             pose->orientation.w = 1;
             pose->orientation.x = 0;
             pose->orientation.y = 0;
             pose->orientation.z = 0;
+        }
         pose->position.x = convertFromString<double>(parts[0]);
         pose->position.y = convertFromString<double>(parts[1]);
         pose->position.z = convertFromString<double>(parts[2]);

src/btree/src/Tree.cpp

@@ -8,7 +8,6 @@
 #include <rclcpp/utilities.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include "nodes/WeightedRandomNode.h"
-#include "nodes/AmICalled.h"
 #include "nodes/GenerateXYPose.h"
 #include "nodes/RobotMove.h"
 #include "nodes/OffsetPose.h"
@@ -40,7 +39,6 @@ int main(int argc, char **argv) {
     std::cout << "Registering nodes." << std::endl;
 
     factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
-    factory.registerNodeType<AmICalled>("AmICalled");
 
     factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
     factory.registerNodeType<OffsetPose>("OffsetPose");

src/btree/src/nodes/ActorAnimation.cpp

@@ -12,7 +12,8 @@ ActorAnimation::ActorAnimation(const std::string &name, const BT::NodeConfigurat
 
 BT::PortsList ActorAnimation::providedPorts() {
     return {
-        BT::InputPort<std::string>("animation"),
+        BT::InputPort<std::string>("animation_name"),
+        BT::InputPort<float>("animation_speed"),
     };
 }
 
@@ -21,15 +22,16 @@ BT::NodeStatus ActorAnimation::onStart() {
 
     ros_actor_action_server_msgs::action::Animation::Goal goal;
 
-    auto res = getInput<std::string>("animation", goal.animation_name);
-
-    if (!res) {
+    if (!getInput<std::string>("animation_name", goal.animation_name)) {
         std::cerr << "[ no animation name specified ]" << std::endl;
-        std::cout << res.error() << std::endl;
         return BT::NodeStatus::FAILURE;
     }
     
+    if (!getInput<float>("animation_speed", goal.animation_speed)) {
         goal.animation_speed=1.0;
+    }
+
+    
 
     auto send_goal_options = Client<Animation>::SendGoalOptions();
     send_goal_options.result_callback = [=](const ClientGoalHandle<Animation>::WrappedResult & parameter) {
@@ -44,12 +46,16 @@ BT::NodeStatus ActorAnimation::onStart() {
 
     client->async_send_goal(goal,send_goal_options);
 
+    result = BT::NodeStatus::RUNNING;
     return BT::NodeStatus::RUNNING;
 }
 
 BT::NodeStatus ActorAnimation::onRunning() {
     mutex.lock();
     auto status = result;
+    if(result != BT::NodeStatus::RUNNING){
+        result = BT::NodeStatus::IDLE;
+    }
     mutex.unlock();
     return status;
 }

src/btree/src/nodes/ActorMovement.cpp

@@ -16,7 +16,8 @@ ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguratio
 BT::PortsList ActorMovement::providedPorts() {
     return {
         BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target"),
-        BT::InputPort<std::string>("animation"),
+        BT::InputPort<std::string>("animation_name"),
+        BT::InputPort<float>("animation_distance"),
     };
 }
 
@@ -25,19 +26,23 @@ BT::NodeStatus ActorMovement::onStart() {
 
     ros_actor_action_server_msgs::action::Movement::Goal goal;
 
+    {
         std::shared_ptr<geometry_msgs::msg::Pose> target;
-    auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target);
-
-    if (!res) {
-        std::cout << "[ no target available ]" << std::endl;
-        std::cout << res.error() << std::endl;
+        if (!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target)) {
+            std::cout << "[ no target given ]" << std::endl;
             return BT::NodeStatus::FAILURE;
         }
-
-    goal.animation_distance=1.5;
-    goal.animation_name="walk";
-    goal.animation_speed=1.0;
         goal.target = *target;
+    }
+    
+    if(!getInput<float>("animation_distance", goal.animation_distance)){
+        goal.animation_distance = 1.0f;
+    }
+    
+    if(!getInput<std::string>("animation_name", goal.animation_name)){
+        std::cout << "[ no animation_name given ]" << std::endl;
+        return BT::NodeStatus::FAILURE;
+    }
 
     auto send_goal_options = Client<Movement>::SendGoalOptions();
     send_goal_options.feedback_callback = [&](__attribute__((unused)) std::shared_ptr<ClientGoalHandle<Movement>> goal_handle, std::shared_ptr<const Movement::Feedback> feedback) {

src/btree/src/nodes/GenerateXYPose.cpp

@@ -77,6 +77,11 @@ BT::NodeStatus BT::GenerateXYPose::tick() {
     auto randomPose = std::make_shared<geometry_msgs::msg::Pose>();
     randomPose->position.x = pos.x;
     randomPose->position.y = pos.y;
+    randomPose->position.z = 0;
+    randomPose->orientation.w = 0;
+    randomPose->orientation.x = 0;
+    randomPose->orientation.y = 0;
+    randomPose->orientation.z = 0;
 
     printf("Generated Target: %f %f\n", pos.x, pos.y);
     setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", randomPose);

src/btree/src/nodes/OffsetPose.cpp

@@ -3,14 +3,16 @@
 //
 
 #include "OffsetPose.h"
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <memory>
 
 
 BT::PortsList BT::OffsetPose::providedPorts() {
     return {
-            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("input", "initial position as Position2D"),
+            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("input", "initial position as Pose"),
             InputPort<std::shared_ptr<geometry_msgs::msg::Point>>("offset", "offset as a Point"),
             InputPort<std::shared_ptr<geometry_msgs::msg::Quaternion>>("orientation", "rotation of resulting pose as Quaternion"),
-            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output", "generated new pose")
+            OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output", "generated new pose")
     };
 }
 
@@ -24,23 +26,24 @@ BT::NodeStatus BT::OffsetPose::tick() {
         return NodeStatus::FAILURE;
     }
     
+    auto resultPose = std::make_shared<geometry_msgs::msg::Pose>(*pose);
+
     std::shared_ptr<geometry_msgs::msg::Quaternion> quaternion;
     if(getInput("orientation", quaternion)) {
-        pose->orientation.w = quaternion->w;
-        pose->orientation.x = quaternion->x;
-        pose->orientation.y = quaternion->y;
-        pose->orientation.z = quaternion->z;
+        resultPose->orientation.w = quaternion->w;
+        resultPose->orientation.x = quaternion->x;
+        resultPose->orientation.y = quaternion->y;
+        resultPose->orientation.z = quaternion->z;
     }
 
     std::shared_ptr<geometry_msgs::msg::Point> offset;
     if(getInput("offset", offset)) {
-        pose->position.x += offset->x;
-        pose->position.y += offset->y;
-        pose->position.z += offset->z;
+        resultPose->position.x += offset->x;
+        resultPose->position.y += offset->y;
+        resultPose->position.z += offset->z;
     }
 
-    setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("output", pose);
+    setOutput("output", resultPose);
 
     return BT::NodeStatus::SUCCESS;
 }
-

src/btree/src/nodes/RandomFailure.cpp

@@ -6,7 +6,7 @@
 
 BT::NodeStatus BT::RandomFailure::tick() {
     double failureChance;
-    if (!getInput<double>("failureChance", failureChance)) {
+    if (!getInput<double>("failure_chance", failureChance)) {
         std::cout << "could not get failure chance." << std::endl;
         return NodeStatus::FAILURE;
     }
@@ -19,7 +19,7 @@ BT::NodeStatus BT::RandomFailure::tick() {
 }
 
 BT::PortsList BT::RandomFailure::providedPorts() {
-    return {InputPort<double>("failureChance", "Chance to fail from 0 to 1")};
+    return {InputPort<double>("failure_chance", "Chance to fail from 0 to 1")};
 }
 
 BT::RandomFailure::RandomFailure(

src/btree/src/nodes/RobotMove.cpp

@@ -13,7 +13,7 @@ BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &c
 BT::NodeStatus BT::RobotMove::onStart() {
     std::cout << "Starting RobotMove" << std::endl;
     std::shared_ptr<geometry_msgs::msg::Pose> pose;
-    if(!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target",pose)){
+    if(!getInput("target",pose)){
         std::cout << "no target available." << std::endl;
         return NodeStatus::FAILURE;
     }
@@ -22,8 +22,10 @@ BT::NodeStatus BT::RobotMove::onStart() {
     asyncResult = NodeStatus::RUNNING;
     connection->get()->planAndExecute(pose,[this](bool finished){
         if(finished){
+            printf("Finished move.");
             asyncResult = NodeStatus::SUCCESS;
         } else {
+            printf("Failed move.");
             asyncResult = NodeStatus::FAILURE;
         }
     });

src/btree/trees/actorTreeCoex.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="actorTree">
+    <BehaviorTree ID="actorTree">
+        <Control ID="WeightedRandom" weights="95,5">
+            <Sequence>
+                <Control ID="WeightedRandom" weights="95,5">
+                    <Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
+                    <Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
+                </Control>
+                <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
+                <Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
+                <Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
+            </Sequence>
+            <Sequence>
+                <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
+                <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
+            </Sequence>
+        </Control>
+    </BehaviorTree>
+</root>

src/btree/trees/actorTreeColab.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<root main_tree_to_execute="BehaviorTree">
+<root main_tree_to_execute="actorTree">
     <!-- ////////// -->
-    <BehaviorTree ID="BehaviorTree">
+    <BehaviorTree ID="actorTree">
         <Fallback>
             <ReactiveSequence>
                 <Inverter>
@@ -14,12 +14,12 @@
                         <Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
                         <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
                     </Control>
-                    <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
+                    <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
                 </Sequence>
             </ReactiveSequence>
             <Sequence>
                 <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
-                <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
+                <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
                 <Action ID="SetCalledTo" state="false"/>
             </Sequence>
         </Fallback>

src/btree/trees/actorTreeCoop.xml

@@ -0,0 +1,73 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="actorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="actorTree">
+        <Fallback>
+            <ReactiveSequence>
+                <Inverter>
+                    <Condition ID="IsCalled"/>
+                </Inverter>
+
+                <Sequence>
+                    <Control ID="WeightedRandom" weights="100,5,2">
+                        <Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
+                        <Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
+                        <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
+                    </Control>
+                    <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
+                </Sequence>
+            </ReactiveSequence>
+            <Sequence>
+                <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
+                <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
+                <Action ID="SetCalledTo" state="false"/>
+            </Sequence>
+        </Fallback>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="ActorAnimation">
+            <input_port name="animation">Name of animation to play</input_port>
+        </Action>
+        <Action ID="ActorMovement">
+            <input_port name="animation">Name of animation to play</input_port>
+            <input_port name="target">Pose to move to</input_port>
+        </Action>
+        <Condition ID="IsCalled"/>
+        <Action ID="SetCalledTo">
+            <input_port name="state">state to set called to</input_port>
+        </Action>
+        <Action ID="GenerateXYPose">
+            <input_port name="area">Area to generate pose in</input_port>
+            <output_port name="pose">Generated pose in area</output_port>
+        </Action>
+        <Condition ID="InAreaTest">
+            <input_port name="area">Bounds to check in</input_port>
+            <input_port name="pose">Position of object</input_port>
+        </Condition>
+        <Action ID="MoveActorToTarget">
+            <input_port name="current">Current actor position</input_port>
+            <input_port name="target">Target to move actor to</input_port>
+        </Action>
+        <Action ID="PositionToPose">
+            <input_port name="offset">offset as a Point</input_port>
+            <input_port name="orientation">rotation of resulting pose as Quaternion</input_port>
+            <input_port name="output">generated new pose</input_port>
+            <input_port name="position">initial position as Position2D</input_port>
+        </Action>
+        <Action ID="SetRobotVelocity">
+            <input_port name="velocity">Target velocity of robot</input_port>
+        </Action>
+        <Action ID="RandomFailure">
+            <input_port name="failureChance">Chance to fail from 0 to 1</input_port>
+        </Action>
+        <Action ID="RobotMove">
+            <input_port name="target">Target pose of robot.</input_port>
+        </Action>
+        <Control ID="WeightedRandom">
+            <input_port name="weights">Weights for the children, separated by semicolon.</input_port>
+        </Control>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
+

src/btree/trees/robotTreeCoex.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="robotTree">
+    <BehaviorTree ID="robotTree">
+        <ReactiveSequence>
+            <Inverter>
+                <Condition ID="InAreaTest" area="{unsafeArea}" pose="{actorPos}"/>
+            </Inverter>
+            <IfThenElse>
+                <Condition ID="InAreaTest" area="{warningArea}" pose="{actorPos}"/>
+                <Action ID="SetRobotVelocity" velocity="0.1"/>
+                <Action ID="SetRobotVelocity" velocity="1"/>
+            </IfThenElse>
+            <Fallback>
+                <Control ID="InterruptableSequence">
+                    <Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/>
+                    <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
+                    <Action ID="RobotMove" target="{target}"/>
+                    <Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/>
+                    <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
+                    <Action ID="RobotMove" target="{target}"/>
+                </Control>
+            </Fallback>
+        </ReactiveSequence>
+    </BehaviorTree>
+</root>

src/btree/trees/robotTreeColab.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<root main_tree_to_execute="BehaviorTree">
+<root main_tree_to_execute="robotTree">
     <!-- ////////// -->
-    <BehaviorTree ID="BehaviorTree">
+    <BehaviorTree ID="robotTree">
         <ReactiveSequence>
             <Inverter>
                 <Condition ID="InAreaTest" area="{unsafeArea}" pose="{actorPos}"/>
@@ -16,11 +16,11 @@
                         <Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/>
                         <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
                         <Action ID="RobotMove" target="{target}"/>
-                        <Action ID="RandomFailure" failureChance="0.15"/>
+                        <Action ID="RandomFailure" failure_chance="0.15"/>
                         <Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/>
                         <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
                         <Action ID="RobotMove" target="{target}"/>
-                        <Action ID="RandomFailure" failureChance="0.05"/>
+                        <Action ID="RandomFailure" failure_chance="0.05"/>
                 </Control>
                 <Action ID="SetCalledTo" state="true"/>
             </Fallback>

src/btree/trees/robotTreeCoop.xml

@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="robotTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="robotTree">
+        <ReactiveSequence>
+            <Inverter>
+                <Condition ID="InAreaTest" area="{unsafeArea}" pose="{actorPos}"/>
+            </Inverter>
+            <IfThenElse>
+                <Condition ID="InAreaTest" area="{warningArea}" pose="{actorPos}"/>
+                <Action ID="SetRobotVelocity" velocity="0.1"/>
+                <Action ID="SetRobotVelocity" velocity="1"/>
+            </IfThenElse>
+            <Control ID="InterruptableSequence">
+                <Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/>
+                <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
+                <Action ID="RobotMove" target="{target}"/>
+                <Control ID="WeightedRandom" weights="90,10">
+                    <Sequence>
+                        <Action ID="OffsetPose" input="-0.68,0,1.215" offset="0,0,0" orientation="0,-0.707,0,0.707" output="{target}"/>
+                        <Action ID="RobotMove" target="{target}"/>
+                    </Sequence>
+                    <Sequence>
+                        <Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/>
+                        <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
+                        <Action ID="RobotMove" target="{target}"/>
+                        <Action ID="SetCalledTo" state="true"/>
+                    </Sequence>
+                </Control>
+            </Control>
+        </ReactiveSequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="ActorAnimation">
+            <input_port name="animation">Name of animation to play</input_port>
+        </Action>
+        <Action ID="ActorMovement">
+            <input_port name="animation">Name of animation to play</input_port>
+            <input_port name="target">Pose to move to</input_port>
+        </Action>
+        <Condition ID="IsCalled"/>
+        <Action ID="SetCalledTo">
+            <input_port name="state">state to set called to</input_port>
+        </Action>
+        <Action ID="GenerateXYPose">
+            <input_port name="area">Area to generate pose in</input_port>
+            <output_port name="pose">Generated pose in area</output_port>
+        </Action>
+        <Condition ID="InAreaTest">
+            <input_port name="area">Bounds to check in</input_port>
+            <input_port name="pose">Position of object</input_port>
+        </Condition>
+        <Action ID="OffsetPose">
+            <input_port name="input">initial position as Pose</input_port>
+            <input_port name="offset">offset as a Point</input_port>
+            <input_port name="orientation">rotation of resulting pose as Quaternion</input_port>
+            <output_port name="output">generated new pose</output_port>
+        </Action>
+        <Action ID="RandomFailure">
+            <input_port name="failureChance">Chance to fail from 0 to 1</input_port>
+        </Action>
+        <Action ID="RobotMove">
+            <input_port name="target">Target pose of robot.</input_port>
+        </Action>
+        <Action ID="SetRobotVelocity">
+            <input_port name="velocity">Target velocity of robot</input_port>
+        </Action>
+        <Control ID="WeightedRandom">
+            <input_port name="weights">Weights for the children, separated by semicolon.</input_port>
+        </Control>
+        <Control ID="InterruptableSequence">
+        </Control>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
+

src/btree_nodes/plugins.xml

@@ -1,5 +0,0 @@
-<library path="btree_nodes">
-    <class type="Factory" base_class_type="BT::BehaviorTreeFactory">
-        <description>Ye.</description>
-    </class>
-</library>

src/btree_nodes/src/Factory.cpp

@@ -1,16 +0,0 @@
-//
-// Created by bastian on 18.08.22.
-//
-
-#include "Factory.h"
-
-
-BT::ActorNodeFactory::ActorNodeFactory() {
-    auto YES = [](BT::TreeNode &parent_node) -> BT::NodeStatus {
-        return BT::NodeStatus::SUCCESS;
-    };
-
-    registerSimpleCondition("YES!", YES);
-}
-
-PLUGINLIB_EXPORT_CLASS(BT::ActorNodeFactory, BT::BehaviorTreeFactory)

src/btree_nodes/src/Factory.h

@@ -1,18 +0,0 @@
-//
-// Created by bastian on 18.08.22.
-//
-
-#ifndef BUILD_FACTORY_H
-#define BUILD_FACTORY_H
-
-#include <behaviortree_cpp_v3/bt_factory.h>
-#include <pluginlib/class_list_macros.hpp>
-
-namespace BT{
-    class ActorNodeFactory : public BT::BehaviorTreeFactory {
-    public:
-        ActorNodeFactory();
-    };
-}
-
-#endif //BUILD_FACTORY_H

src/btree_nodes/src/nodes/AmICalled.cpp

@@ -1,23 +0,0 @@
-//
-// Created by bastian on 26.03.22.
-//
-
-#include "AmICalled.h"
-
-BT::AmICalled::AmICalled(const std::string &name, const BT::NodeConfiguration &config) :
-        ConditionNode(name, config) {}
-
-BT::PortsList BT::AmICalled::providedPorts() {
-    return {InputPort<bool>("called")};
-}
-
-BT::NodeStatus BT::AmICalled::tick() {
-    bool called;
-    if (!getInput("called", called)) {
-        return NodeStatus::FAILURE;
-    }
-    if (called) {
-        return NodeStatus::SUCCESS;
-    }
-    return NodeStatus::FAILURE;
-}

src/btree_nodes/src/nodes/AmICalled.h

@@ -1,22 +0,0 @@
-//
-// Created by bastian on 26.03.22.
-//
-
-#ifndef BUILD_AMICALLED_H
-#define BUILD_AMICALLED_H
-
-#include <behaviortree_cpp_v3/condition_node.h>
-
-namespace BT {
-
-    class AmICalled : public ConditionNode {
-    public:
-        AmICalled(const std::string &name, const NodeConfiguration &config);
-
-        static PortsList providedPorts();
-
-        NodeStatus tick() override;
-    };
-
-}
-#endif //BUILD_AMICALLED_H

src/btree_trees/package.xml

@@ -1,13 +0,0 @@
-<package format="3">
-  <name>btree_trees</name>
-  <version>0.244.1</version>
-  <description>Trees for the simulation</description>
-  <license>Apache 2.0</license>
-  <maintainer email="bastian.hofmann@xitaso.com">Bastian Hofmann</maintainer>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/controller_test/CMakeLists.txt

@@ -1,17 +1,28 @@
 cmake_minimum_required(VERSION 3.8)
-project(btree_trees)
+project(controller_test)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(control_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-install(DIRECTORY   trees     DESTINATION   share/${PROJECT_NAME})
+add_executable(move src/test.cpp)
+set_property(TARGET move PROPERTY CXX_STANDARD 17)
+ament_target_dependencies(move rclcpp moveit_ros_planning_interface control_msgs)
+
+install(TARGETS
+        move
+        DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

src/controller_test/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>controller_test</name>
+  <version>0.1.0</version>
+  <description>
+     Test for Moveit on IISY
+  </description>
+  <author email="bastian.hofmann@xitaso.com">bastian</author>
+  <maintainer email="bastian.hofmann@xitaso.com">bastian</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

src/controller_test/src/test.cpp

@@ -0,0 +1,203 @@
+#include <control_msgs/action/detail/follow_joint_trajectory__struct.hpp>
+#include <memory>
+#include <rclcpp_action/client_goal_handle.hpp>
+#include <rclcpp_action/types.hpp>
+#include <string>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include <control_msgs/action/follow_joint_trajectory.hpp>
+
+std::shared_ptr<rclcpp::Node> node;
+
+void common_goal_response(
+  const rclcpp_action::ClientGoalHandle
+  <control_msgs::action::FollowJointTrajectory>::SharedPtr& future)
+{
+  RCLCPP_DEBUG(
+    node->get_logger(), "common_goal_response time: %f",
+    rclcpp::Clock().now().seconds());
+  auto goal_handle = future.get();
+  if (!goal_handle) {
+    printf("Goal rejected\n");
+  } else {
+    printf("Goal accepted\n");
+  }
+}
+
+void common_result_response(
+  const rclcpp_action::ClientGoalHandle
+  <control_msgs::action::FollowJointTrajectory>::WrappedResult & result)
+{
+  printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds());
+  switch (result.code) {
+    case rclcpp_action::ResultCode::SUCCEEDED:
+      printf("SUCCEEDED result code\n");
+      break;
+    case rclcpp_action::ResultCode::ABORTED:
+      printf("Goal was aborted\n");
+      return;
+    case rclcpp_action::ResultCode::CANCELED:
+      printf("Goal was canceled\n");
+      return;
+    default:
+      printf("Unknown result code\n");
+      return;
+  }
+}
+
+void common_feedback(
+  const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr&,
+  const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback>& feedback)
+{
+  std::cout << "feedback->desired.positions :";
+  for (auto & x : feedback->desired.positions) {
+    std::cout << x << "\t";
+  }
+  std::cout << std::endl;
+  std::cout << "feedback->desired.velocities :";
+  for (auto & x : feedback->desired.velocities) {
+    std::cout << x << "\t";
+  }
+  std::cout << std::endl;
+}
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  node = std::make_shared<rclcpp::Node>("trajectory_test_node");
+
+  std::cout << "node created" << std::endl;
+
+  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
+  action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
+    node->get_node_base_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_logging_interface(),
+    node->get_node_waitables_interface(),
+    "/arm_controller/follow_joint_trajectory");
+
+  bool response =
+    action_client->wait_for_action_server(std::chrono::seconds(10));
+  if (!response) {
+    throw std::runtime_error("could not get action server");
+  }
+  std::cout << "Created action server" << std::endl;
+
+  std::vector<std::string> joint_names = {
+    "base_rot",
+    "base_link1_joint",
+    "link1_link2_joint",
+    "link2_rot",
+    "link2_link3_joint",
+    "link3_rot"
+    };
+
+  std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
+  trajectory_msgs::msg::JointTrajectoryPoint point;
+  point.time_from_start = rclcpp::Duration::from_seconds(0.0);  // start asap
+  point.positions.resize(joint_names.size());
+
+  point.positions[0] = 0.0;
+  point.positions[1] = 0.0;
+  point.positions[2] = 0.0;
+  point.positions[3] = 0.0;
+  point.positions[4] = 0.0;
+  point.positions[5] = 0.0;
+
+  trajectory_msgs::msg::JointTrajectoryPoint point2;
+  point2.time_from_start = rclcpp::Duration::from_seconds(1.0);
+  point2.positions.resize(joint_names.size());
+
+  point2.positions[0] = -1.0;
+  point2.positions[1] = 0.0;
+  point2.positions[2] = 0.0;
+  point2.positions[3] = 0.0;
+  point2.positions[4] = 0.0;
+  point2.positions[5] = 0.0;
+
+  trajectory_msgs::msg::JointTrajectoryPoint point3;
+  point3.time_from_start = rclcpp::Duration::from_seconds(2.0);
+  point3.positions.resize(joint_names.size());
+
+  point3.positions[0] = 1.0;
+  point3.positions[1] = 0.0;
+  point3.positions[2] = 0.0;
+  point3.positions[3] = 0.0;
+  point3.positions[4] = 0.0;
+  point3.positions[5] = 0.0;
+
+  trajectory_msgs::msg::JointTrajectoryPoint point4;
+  point4.time_from_start = rclcpp::Duration::from_seconds(3.0);
+  point4.positions.resize(joint_names.size());
+
+  point4.positions[0] = 0.0;
+  point4.positions[1] = 0.0;
+  point4.positions[2] = M_PI/2;
+  point4.positions[3] = 0.0;
+  point4.positions[4] = 0.0;
+  point4.positions[5] = 0.0;
+
+  points.push_back(point);
+  points.push_back(point2);
+  points.push_back(point3);
+  points.push_back(point4);
+
+  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
+  opt.goal_response_callback = [](std::shared_ptr<rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>> goal){ 
+    if(goal->get_status()==rclcpp_action::GoalStatus::STATUS_ACCEPTED){
+      RCLCPP_DEBUG(
+        node->get_logger(), "common_goal_response accepted: %f",
+        rclcpp::Clock().now().seconds());
+    }else{
+      RCLCPP_DEBUG(
+        node->get_logger(), "common_goal_response rejected: %f",
+        rclcpp::Clock().now().seconds());
+    }
+  };
+  //opt.goal_response_callback = [](auto && PH1) { return common_goal_response(std::forward<decltype(PH1)>(PH1)); };
+  opt.result_callback = [](auto && PH1) { return common_result_response(std::forward<decltype(PH1)>(PH1)); };
+  opt.feedback_callback = [](auto && PH1, auto && PH2) { return common_feedback(std::forward<decltype(PH1)>(PH1), std::forward<decltype(PH2)>(PH2)); };
+
+  control_msgs::action::FollowJointTrajectory_Goal goal_msg;
+  goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
+  goal_msg.trajectory.joint_names = joint_names;
+  goal_msg.trajectory.points = points;
+
+  auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);
+
+  if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
+    return 1;
+  }
+  RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
+
+  const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr&
+    goal_handle = goal_handle_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
+    return 1;
+  }
+  RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server");
+
+  // Wait for the server to be done with the goal
+  auto result_future = action_client->async_get_result(goal_handle);
+  RCLCPP_INFO(node->get_logger(), "Waiting for result");
+  if (rclcpp::spin_until_future_complete(node, result_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
+    return 1;
+  }
+
+  std::cout << "async_send_goal" << std::endl;
+  
+  rclcpp::shutdown();
+
+  return 0;
+}

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -125,10 +125,11 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         auto newTime = animationTimeComponent->Data() + _info.dt;
         auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
         
-        feedback.progress = newTimeSeconds / duration;
+        auto progress = newTimeSeconds / (duration/animation_speed);
+        feedback.progress = progress;
         sendFeedback();
 
-        if (newTimeSeconds >= duration) {
+        if (progress >= 1.0) {
             igndbg << "Animation " << animation_name << " finished." << std::endl;
             nextState = IDLE;
             feedback.progress = 0.0f;
@@ -166,7 +167,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
             if(movementDetails.targetDiff.Pos().Length() >= 0.001){
                 movementDetails.targetDiff.Rot() = ignition::math::Quaterniond::EulerToQuaternion(0.0, 0.0, atan2(movementDetails.targetDiff.Pos().Y(), movementDetails.targetDiff.Pos().X()));
                 movementDetails.rotateToTargetDuration = Angle(actorPose.Rot(),movementDetails.targetDiff.Rot()) / turnSpeed;
-                movementDetails.moveDuration = movementDetails.targetDiff.Pos().Length() / animation_distance * duration;
+                movementDetails.moveDuration = movementDetails.targetDiff.Pos().Length() / animation_speed * duration;
             }else{
                 movementDetails.targetDiff.Rot() = movementDetails.start.Rot();
                 movementDetails.rotateToTargetDuration = 0.0;
@@ -190,6 +191,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
 
         if (movementDetails.state == movementDetails.ROTATE_TO_TARGET){
             if(movementDetails.stepTime<=movementDetails.rotateToTargetDuration){
+                newTime = newTime.zero();
                 actorPose.Rot() = ignition::math::Quaterniond::Slerp(movementDetails.stepTime/movementDetails.rotateToTargetDuration,movementDetails.start.Rot(),movementDetails.targetDiff.Rot(),true);
             }else{
                 actorPose.Rot() = movementDetails.targetDiff.Rot();
@@ -311,8 +313,7 @@ void ActorSystem::messageQueueInterface(const char name[256]) {
             threadMutex.lock();
 
             strcpy(animation_name, receivedAction.animationName);
-            // animation_speed = receivedAction.animationSpeed;
-            animation_distance = receivedAction.animationDistance;
+            animation_speed = receivedAction.animationSpeed;
             movementDetails.target.Pos().Set(receivedAction.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z);
             movementDetails.target.Rot().Set(receivedAction.target.orientation.w, receivedAction.target.orientation.x, receivedAction.target.orientation.y, receivedAction.target.orientation.z);
             nextState = receivedAction.state;

src/ign_actor_plugin/src/ActorSystem.hpp

@@ -59,7 +59,7 @@ class ActorSystem : public System,
     private:
         ignition::math::Pose3d startPose = ignition::math::Pose3d::Zero;
         ignition::math::Pose3d target_pose;
-        double animation_distance;
+        double animation_speed;
         char animation_name[256];
         ActorPluginState nextState,currentState,lastState;
         double duration{};

src/ign_world/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(ament_cmake REQUIRED)
 # find_package(<dependency> REQUIRED)
 
 install(DIRECTORY   launch    DESTINATION   share/${PROJECT_NAME})
+install(DIRECTORY   model     DESTINATION   share/${PROJECT_NAME})
 install(DIRECTORY   rviz      DESTINATION   share/${PROJECT_NAME})
 install(DIRECTORY   world     DESTINATION   share/${PROJECT_NAME})
 

src/ign_world/launch/gazebo_controller_launch.py

@@ -32,36 +32,15 @@ def generate_launch_description():
         output='screen'
     )
     
-    return LaunchDescription([
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')),
-            launch_arguments={'gz_args': '-v 4 -r '+get_package_share_directory('ign_world')+'/world/gaz_new_test.sdf'}.items(),
-        ),
-
-        Node(
-            package='ros_gz_bridge',
-            executable='parameter_bridge',
-            arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
-            output='both',
-        ),
-
-        SetParameter(name="use_sim_time", value=True),
-
-        Node(
-            package='ros_actor_action_server',
-            executable='ros_actor_action_server',
-            output='both',
-        ),
-        
-        Node(
-            package='btree_nodes',
+    btree = Node(
+        package='btree',
         executable='tree',
         output='both',
         parameters=[
             {
                 "trees": [
-                        get_package_share_directory('btree_trees')+'/trees/actorTree.xml',
-                        get_package_share_directory('btree_trees')+'/trees/robotTree.xml'
+                    get_package_share_directory('btree')+'/trees/actorTreeCoex.xml',
+                    get_package_share_directory('btree')+'/trees/robotTreeCoex.xml'
                 ],
                 "areas": [
                     "safeArea",
@@ -95,9 +74,46 @@ def generate_launch_description():
                     "0,     0.4"
                 ]
             },
-            ]
+        ],
+        emulate_tty=True,
+    )
+
+    return LaunchDescription([
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')),
+            launch_arguments={'gz_args': '-v 4 -r '+get_package_share_directory('ign_world')+'/world/gaz_new_test.sdf'}.items(),
         ),
         
+        Node(
+            package='ros_gz_sim',
+            executable='create',
+            arguments=[
+                '-name', 'office',
+                '-allow_renaming', 'true',
+                '-string', xacro.process(os.path.join(get_package_share_directory('ign_world'), 'world', 'conveyor.sdf')),
+                #'-file', 'https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Gazebo',
+            ],
+            output='screen'
+        ),
+
+        Node(
+            package='ros_gz_bridge',
+            executable='parameter_bridge',
+            arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
+            output='both',
+        ),
+
+        SetParameter(name="use_sim_time", value=True),
+
+        Node(
+            package='ros_actor_action_server',
+            executable='ros_actor_action_server',
+            output='both',
+            emulate_tty=True,
+        ),
+        
+        btree,
+
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
                 str(moveit_config.package_path / "launch/rsp.launch.py")
@@ -116,6 +132,7 @@ def generate_launch_description():
                 on_exit=[load_joint_state_broadcaster],
             )
         ),
+        
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 target_action=load_joint_state_broadcaster,

src/ign_world/world/conveyor.sdf

@@ -0,0 +1,19 @@
+<sdf version="1.6">
+    <model name="office">
+        <static>true</static>
+        <link name="office">
+            <visual name="visual">
+                <geometry>
+                    <mesh>
+                        <uri>file://$(find ign_world)/model/conveyor.stl</uri>
+                    </mesh>
+                </geometry>
+                <material>
+                    <ambient>0.8 0.8 0.8 1</ambient>
+                    <diffuse>0.8 0.8 0.8 1</diffuse>
+                    <specular>0.8 0.8 0.8 1</specular>
+                </material>
+            </visual>
+        </link>
+    </model>
+</sdf>

src/iisy_config/config/iisy.urdf

@@ -5,29 +5,29 @@
   <link name="table">
     <collision>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/table.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/table.stl"/>
       </geometry>
     </collision>
     <visual>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/table.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/table.stl"/>
       </geometry>
     </visual>
   </link>
   <link name="base_bottom">
     <collision>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/base_bottom_coll.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/base_bottom_coll.stl"/>
       </geometry>
     </collision>
     <visual>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/base_bottom_low.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/base_bottom_low.stl"/>
       </geometry>
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="-0.043517 0.000000 0.054914"/>
-      <mass value="0.1"/>
+      <mass value="6"/>
       <inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
     </inertial>
   </link>
@@ -40,29 +40,29 @@
     </collision>
     <visual>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/base_top_low.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/base_top_low.stl"/>
       </geometry>
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.005 0.08"/>
-      <mass value="0.1"/>
+      <mass value="2"/>
       <inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/>
     </inertial>
   </link>
   <link name="link1_full">
     <collision>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/link_1_full_coll.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/link_1_full_coll.stl"/>
       </geometry>
     </collision>
     <visual>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/link_1_full_low.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/link_1_full_low.stl"/>
       </geometry>
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.050000 0.150000"/>
-      <mass value="0.1"/>
+      <mass value="4"/>
       <inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/>
     </inertial>
   </link>
@@ -75,63 +75,63 @@
     </collision>
     <visual>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/link_2_bottom_low.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/link_2_bottom_low.stl"/>
       </geometry>
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0.07 0.025"/>
-      <mass value="0.1"/>
+      <mass value="2"/>
       <inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/>
     </inertial>
   </link>
   <link name="link2_top">
     <collision>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/link_2_top_coll.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/link_2_top_coll.stl"/>
       </geometry>
     </collision>
     <visual>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/link_2_top_low.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/link_2_top_low.stl"/>
       </geometry>
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0.037363 0.086674"/>
-      <mass value="0.1"/>
+      <mass value="2"/>
       <inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/>
     </inertial>
   </link>
   <link name="link3_bottom">
     <collision>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/link_3_bottom_coll.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/link_3_bottom_coll.stl"/>
       </geometry>
     </collision>
     <visual>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/link_3_bottom_low.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/link_3_bottom_low.stl"/>
       </geometry>
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.055000 0.021413"/>
-      <mass value="0.1"/>
+      <mass value="2"/>
       <inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/>
     </inertial>
   </link>
   <link name="link3_top">
     <collision>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/link_3_top_coll.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/link_3_top_coll.stl"/>
       </geometry>
     </collision>
     <visual>
       <geometry>
-        <mesh filename="$(find iisy_config)/stl/link_3_top_low.stl"/>
+        <mesh filename="file://$(find iisy_config)/stl/link_3_top_low.stl"/>
       </geometry>
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.015462 0.040000"/>
-      <mass value="0.1"/>
+      <mass value="0.8"/>
       <inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
     </inertial>
   </link>
@@ -155,31 +155,31 @@
     <parent link="base_bottom"/>
     <child link="base_top"/>
     <axis xyz="0 0 1"/>
-    <limit effort="30" velocity="1.7453292519943"/>
-    <dynamics damping="10.0" friction="0.1"/>
+    <limit effort="300" velocity="3.49065850399"/>
+    <dynamics damping="30.0" friction="0.1"/>
   </joint>
   <joint name="base_link1_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 -0.080499 0.092997"/>
     <parent link="base_top"/>
     <child link="link1_full"/>
     <axis xyz="0 1 0"/>
-    <limit effort="30" lower="-2.2689280275926" upper="2.4434609527921" velocity="1.7453292519943"/>
-    <dynamics damping="10.0" friction="0.1"/>
+    <limit effort="300" lower="-2.2689280275926" upper="2.4434609527921" velocity="3.49065850399"/>
+    <dynamics damping="30.0" friction="0.1"/>
   </joint>
   <joint name="link1_link2_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 0 0.3"/>
     <parent link="link1_full"/>
     <child link="link2_bottom"/>
     <axis xyz="0 1 0"/>
-    <limit effort="30" lower="-2.6179938779915" upper="2.6179938779915" velocity="1.7453292519943"/>
-    <dynamics damping="10.0" friction="0.1"/>
+    <limit effort="200" lower="-2.6179938779915" upper="2.6179938779915" velocity="3.49065850399"/>
+    <dynamics damping="20.0" friction="0.1"/>
   </joint>
   <joint name="link2_rot" type="continuous">
     <origin rpy="0 0 0" xyz="0 0.080501 0.120502"/>
     <parent link="link2_bottom"/>
     <child link="link2_top"/>
     <axis xyz="0 0 1"/>
-    <limit effort="30" velocity="1.7453292519943"/>
+    <limit effort="100" velocity="5.23598775598"/>
     <dynamics damping="10.0" friction="0.1"/>
   </joint>
   <joint name="link2_link3_joint" type="continuous">
@@ -187,16 +187,16 @@
     <parent link="link2_top"/>
     <child link="link3_bottom"/>
     <axis xyz="0 1 0"/>
-    <limit effort="30" velocity="1.7453292519943"/>
-    <dynamics damping="10.0" friction="0.1"/>
+    <limit effort="50" velocity="5.23598775598"/>
+    <dynamics damping="5.0" friction="0.1"/>
   </joint>
   <joint name="link3_rot" type="continuous">
     <origin rpy="0 0 0" xyz="0 -0.055001 0.085501"/>
     <parent link="link3_bottom"/>
     <child link="link3_top"/>
     <axis xyz="0 0 1"/>
-    <limit effort="30" velocity="1.7453292519943"/>
-    <dynamics damping="10.0" friction="0.1"/>
+    <limit effort="50" velocity="6.98131700798"/>
+    <dynamics damping="5.0" friction="0.1"/>
   </joint>
   <transmission name="trans_base_rot">
     <type>transmission_interface/SimpleTransmission</type>

src/ros_actor_action_server/src/Server.cpp

@@ -122,6 +122,7 @@ int main(int argc, char **argv) {
             if (currentFeedback.state == IDLE) {
                 std::cout << "Accepting..." << std::endl;
                 
+                currentAction.animationSpeed = animationGoal->animation_speed;
                 strcpy(currentAction.animationName, animationGoal->animation_name.data());
 
                 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
@@ -163,7 +164,7 @@ int main(int argc, char **argv) {
                 std::cout << "Accepting..." << std::endl;
                 
                 currentAction.target = movementGoal->target;
-                currentAction.animationDistance = movementGoal->animation_distance;
+                currentAction.animationSpeed = movementGoal->animation_distance;
                 strcpy(currentAction.animationName, movementGoal->animation_name.data());
 
                 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;

src/ros_actor_action_server_msgs/action/Movement.action

@@ -1,5 +1,4 @@
 string animation_name
-float32 animation_speed
 float32 animation_distance
 geometry_msgs/Pose target
 ---

src/ros_actor_message_queue_msgs/include/ros_actor_message_queue_msgs/MessageTypes.hpp

@@ -1,5 +1,6 @@
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
 namespace ros_actor_message_queue_msgs{
+  
 enum ActorPluginState{
     SETUP,IDLE,ANIMATION,MOVEMENT
 };
@@ -13,7 +14,8 @@ struct FeedbackMessage{
 struct ActionMessage{
     ActorPluginState state;
     geometry_msgs::msg::Pose target;
-  float animationSpeed = 1.0f,animationDistance = 1.0f;
+    float animationSpeed = 1.0f;
     char animationName[256];
 };
+
 }