Visual fixes to lights and robot

.gitmodules

@@ -1,3 +1,3 @@
-[submodule "src/Groot"]
-	path = src/Groot
-	url = https://github.com/BehaviorTree/Groot.git
+[submodule "src/gz_ros2_control"]
+	path = src/gz_ros2_control
+	url = https://github.com/ros-controls/gz_ros2_control.git

README.md

@@ -0,0 +1,21 @@
+# Usage
+`ros2 launch ign_world gazebo_controller_launch.py scene:=` and one of the following scene types: `Coex`, `Coop` and `Colab` appended to the beforementioned command.
+
+Depending on your terminal configuration, adding the environment variable `DISPLAY=:0` may be required as a prefix to the command, as not all terminals load the predefined environment.
+
+# Build
+Building a package is handled by using the `build.sh` executable.
+Many IDE's require a `compile_commands.json` file to offer syntax highlighting, which will be output when using the script. This also includes a symlink, in case your IDE does not search for it in the build directory.
+Another addition of the script is the usage of `console_cohesion+` as event handler, which will format output by package, easing debugging in the noisy build output.
+
+# Packages
+package name|description
+-|-
+`btree` | the executable for the behavior trees and the used xml files
+`controller_test` | a debugging tool for sending direct commands to the ros_control controller of the `iisy`
+`gz_ros2_control` | the git version of that package, as the one in the repos was too outdated
+`ign_actor_plugin` | the Gazebo Ignition plugin for controllable Actors
+`ign_world` | the world for the simulation
+`ros_actor_action_server` | second component of actor plugin, has to be started to enable control functionality
+`ros_actor_action_server_msgs` | ros_action messages used by the actor plugin server
+`ros_actor_message_queue_msgs` | internal messages of the actor plugin

build.sh

@@ -1,4 +1,4 @@
 #!/bin/bash
 pushd "$(dirname "$0")" || exit
-colcon build --event-handlers console_cohesion+ --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja
+colcon build --event-handlers console_cohesion+ --cmake-args -DCMAKE_BUILD_TYPE=Debug -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja
 popd || exit

src/btree/CMakeLists.txt

@@ -1,12 +1,11 @@
 cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
-project(btree_nodes)
+project(btree)
 
 add_compile_options(-Wall -Wextra -Wpedantic)
+set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
-
-
 # find dependencies
 
 set(DEPENDENCIES
@@ -36,10 +35,7 @@ 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
@@ -51,31 +47,17 @@ set(CPP_FILES
         src/nodes/ActorAnimation.cpp
         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_SOURCE_FILES_PROPERTIES(src/Extensions.hpp PROPERTIES COMPILE_FLAGS -O0)
+target_include_directories(tree PUBLIC /src /src/nodes)
 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)
@@ -88,11 +70,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.hpp

@@ -1,12 +1,14 @@
 //
 // Created by bastian on 28.02.22.
 //
+#ifndef BUILD_EXTENSIONS_H
+#define BUILD_EXTENSIONS_H
 
 #include "Area.h"
 #include <geometry_msgs/msg/pose.hpp>
 
 namespace BT {
-    template<>
+    template<> inline
     std::shared_ptr<Position2D> convertFromString(StringView str) {
         auto split = splitString(str, ',');
         if (split.size() != 2) {
@@ -19,7 +21,7 @@ namespace BT {
         return pos;
     }
 
-    template<>
+    template<> inline
     std::shared_ptr<Area> convertFromString(StringView str) {
         auto parts = splitString(str, '|');
         if (parts.size() < 3) {
@@ -35,18 +37,25 @@ namespace BT {
         return output;
     }
 
-    template<>
+    template<> inline
     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>();
-        pose->orientation.w = 1;
-        pose->orientation.x = 0;
-        pose->orientation.y = 0;
-        pose->orientation.z = 0;
+        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]);
@@ -54,7 +63,7 @@ namespace BT {
         return pose;
     }
 
-    template<>
+    template<> inline
     std::shared_ptr<geometry_msgs::msg::Point> convertFromString(StringView str) {
         auto parts = splitString(str, ',');
         if (parts.size() != 3) {
@@ -69,7 +78,7 @@ namespace BT {
         return point;
     }
 
-    template<>
+    template<> inline
     std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str) {
         auto parts = splitString(str, ',');
         if (parts.size() != 4) {
@@ -84,4 +93,6 @@ namespace BT {
 
         return point;
     }
-}
+}
+
+#endif //BUILD_EXTENSIONS_H

src/btree/src/Tree.cpp

@@ -1,14 +1,16 @@
+#include "Extensions.hpp"
 #include "Area.h"
 #include <behaviortree_cpp_v3/basic_types.h>
 #include <behaviortree_cpp_v3/blackboard.h>
 #include <behaviortree_cpp_v3/bt_factory.h>
+#include <boost/exception/exception.hpp>
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <random>
 #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"
@@ -21,9 +23,13 @@
 #include <chrono>
 #include <thread>
 
+using namespace BT;
+
 int main(int argc, char **argv) {
     rclcpp::init(argc, argv);
     rclcpp::NodeOptions node_options;
+    
+    convertFromString<std::shared_ptr<geometry_msgs::msg::Pose>>("0,0,0");
 
     auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options);
     
@@ -40,7 +46,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");
@@ -53,12 +58,12 @@ int main(int argc, char **argv) {
     auto connection = std::make_shared<MoveConnection>(moveNode, "arm");
     executor.add_node(moveNode);
 
-    factory.registerBuilder<RobotMove>("RobotMove", [&connection](const std::string &name, const NodeConfiguration &config) {
-        return std::make_unique<RobotMove>(name, config, &connection);
+    factory.registerBuilder<RobotMove>("RobotMove", [connection](const std::string &name, const NodeConfiguration &config) {
+        return std::make_unique<RobotMove>(name, config, connection);
     });
 
-    factory.registerBuilder<SetRobotVelocity>("SetRobotVelocity", [&connection](const std::string &name, const NodeConfiguration &config) {
-        return std::make_unique<SetRobotVelocity>(name, config, &connection);
+    factory.registerBuilder<SetRobotVelocity>("SetRobotVelocity", [connection](const std::string &name, const NodeConfiguration &config) {
+        return std::make_unique<SetRobotVelocity>(name, config, connection);
     });
 
     

src/btree/src/nodes/ActorAnimation.cpp

@@ -12,31 +12,35 @@ 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"),
     };
 }
 
 BT::NodeStatus ActorAnimation::onStart() {
-    std::cout << "started animation" << std::endl;
+    std::cout << "started actor animation" << std::endl;
 
     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;
+    }
 
-    goal.animation_speed=1.0;
+    
 
     auto send_goal_options = Client<Animation>::SendGoalOptions();
     send_goal_options.result_callback = [=](const ClientGoalHandle<Animation>::WrappedResult & parameter) {
         mutex.lock();
         if(parameter.code == ResultCode::SUCCEEDED){
+            std::cout << "finished actor animation" << std::endl;
             result = BT::NodeStatus::SUCCESS;
         }else{
+            std::cout << "failed actor animation" << std::endl;
             result = BT::NodeStatus::FAILURE;
         }
         mutex.unlock();
@@ -44,6 +48,7 @@ BT::NodeStatus ActorAnimation::onStart() {
 
     client->async_send_goal(goal,send_goal_options);
 
+    result = BT::NodeStatus::RUNNING;
     return BT::NodeStatus::RUNNING;
 }
 
@@ -55,6 +60,7 @@ BT::NodeStatus ActorAnimation::onRunning() {
 }
 
 void ActorAnimation::onHalted() {
-    std::cout << "halted animation" << std::endl;
+    std::cout << "halted actor animation" << std::endl;
     client->async_cancel_all_goals();
+    result = BT::NodeStatus::FAILURE;
 }

src/btree/src/nodes/ActorMovement.cpp

@@ -2,6 +2,7 @@
 // Created by bastian on 26.03.22.
 //
 
+#include "../Extensions.hpp"
 #include "ActorMovement.h"
 #include <action_msgs/msg/detail/goal_status__struct.hpp>
 #include <behaviortree_cpp_v3/basic_types.h>
@@ -16,29 +17,34 @@ 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"),
     };
 }
 
 BT::NodeStatus ActorMovement::onStart() {
-    std::cout << "started moving" << std::endl;
+    std::cout << "started actor movement" << std::endl;
 
     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;
+    {
+        std::shared_ptr<geometry_msgs::msg::Pose> target;
+        if (!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target)) {
+            std::cout << "[ no target given ]" << std::endl;
+            return BT::NodeStatus::FAILURE;
+        }
+        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;
     }
 
-    goal.animation_distance=1.5;
-    goal.animation_name="walk";
-    goal.animation_speed=1.0;
-    goal.target = *target;
-
     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) {
         positionCallback(feedback);
@@ -46,10 +52,10 @@ BT::NodeStatus ActorMovement::onStart() {
     send_goal_options.result_callback = [&](ClientGoalHandle<Movement>::WrappedResult parameter) {
         mutex.lock();
         if(parameter.code == ResultCode::SUCCEEDED){
-            std::cout << "Success?" << std::endl;
+            std::cout << "finished actor movement" << std::endl;
             result = BT::NodeStatus::SUCCESS;
         }else{
-            std::cout << "Failure?" << std::endl;
+            std::cout << "failed actor movement" << std::endl;
             result = BT::NodeStatus::FAILURE;
         }
         mutex.unlock();
@@ -76,15 +82,12 @@ BT::NodeStatus ActorMovement::onStart() {
 BT::NodeStatus ActorMovement::onRunning() {
     mutex.lock();
     auto status = result;
-    if(result != BT::NodeStatus::RUNNING){
-        result = BT::NodeStatus::IDLE;
-    }
     mutex.unlock();
     return status;
 }
 
 void ActorMovement::onHalted() {
-    std::cout << "halted move" << std::endl;
+    std::cout << "halted actor movement" << std::endl;
     client->async_cancel_all_goals();
-    result = BT::NodeStatus::IDLE;
+    result = BT::NodeStatus::FAILURE;
 }

src/btree/src/nodes/GenerateXYPose.cpp

@@ -77,8 +77,14 @@ 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;
+    
+    std::cout << "Generated Pose." << std::endl;
 
-    printf("Generated Target: %f %f\n", pos.x, pos.y);
     setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", randomPose);
     return NodeStatus::SUCCESS;
 }

src/btree/src/nodes/InAreaTest.cpp

@@ -3,6 +3,7 @@
 //
 
 #include "InAreaTest.h"
+#include "../Extensions.hpp"
 
 BT::PortsList BT::InAreaTest::providedPorts() {
     return {

src/btree/src/nodes/InterruptableSequence.cpp

@@ -15,23 +15,24 @@ void BT::InterruptableSequence::halt() {
 BT::NodeStatus BT::InterruptableSequence::tick() {
     auto result = children_nodes_[position]->executeTick();
 
-    setStatus(NodeStatus::RUNNING);
-
     if(result==NodeStatus::FAILURE){
-        haltChildren();
+        resetChildren();
         position=0;
+        setStatus(NodeStatus::FAILURE);
         return NodeStatus::FAILURE;
     }
 
     if(result==NodeStatus::SUCCESS){
         position++;
         if(position>=children_nodes_.size()){
-            haltChildren();
+            resetChildren();
             position=0;
+            setStatus(NodeStatus::SUCCESS);
             return NodeStatus::SUCCESS;
         }
     }
 
+    setStatus(NodeStatus::RUNNING);
     return NodeStatus::RUNNING;
 }
 

src/btree/src/nodes/MoveConnection.cpp

@@ -11,20 +11,19 @@ void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Po
     std::cout<<"planAndExecute."<<std::endl;
     lock.lock();
     state = RUNNING;
-    std::cout<<"got lock."<<std::endl;
     lastTarget = pose;
     lastCallback = callback;
 
     moveGroup.setMaxVelocityScalingFactor(currentSpeed);
     moveGroup.setMaxAccelerationScalingFactor(1);
+    
+    moveGroup.setPoseTarget(*lastTarget);
+    moveGroup.setGoalJointTolerance(0.01);
+    moveGroup.setGoalOrientationTolerance(0.01);
 
     std::cout<<"Starting planning"<<std::endl;
 
     std::thread([&](){
-        moveGroup.setPoseTarget(*lastTarget);
-        moveGroup.setGoalJointTolerance(0.01);
-        moveGroup.setGoalOrientationTolerance(0.01);
-
         std::cout<<"Parameters set."<<std::endl;
         
         auto finished = moveGroup.move() == moveit::core::MoveItErrorCode::SUCCESS;

src/btree/src/nodes/MoveConnection.h

@@ -12,7 +12,7 @@
 
 class MoveConnection {
 private:
-    enum {IDLE,RUNNING,SPEED_CHANGE} state;
+    enum {IDLE,RUNNING,SPEED_CHANGE} state = IDLE;
     moveit::planning_interface::MoveGroupInterface moveGroup;
     std::shared_ptr<geometry_msgs::msg::Pose> lastTarget = nullptr;
     std::function<void(bool)> lastCallback = [](__attribute__((unused))bool finished) {};

src/btree/src/nodes/OffsetPose.cpp

@@ -2,15 +2,18 @@
 // Created by bastian on 29.03.22.
 //
 
+#include "../Extensions.hpp"
 #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")
     };
 }
 
@@ -18,29 +21,31 @@ BT::OffsetPose::OffsetPose(const std::string &name,
         const BT::NodeConfiguration &config) : SyncActionNode(name, config) {}
 
 BT::NodeStatus BT::OffsetPose::tick() {
-
+    
     std::shared_ptr<geometry_msgs::msg::Pose> pose;
     if (!getInput("input", pose)) {
+        std::cout << "Missing {input} pose" << std::endl;
         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

@@ -5,7 +5,7 @@
 #include "RobotMove.h"
 
 BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &config,
-                         std::shared_ptr<MoveConnection> *connection)
+                         std::shared_ptr<MoveConnection> connection)
         : StatefulActionNode(name, config) {
     this->connection = connection;
 }
@@ -13,17 +13,19 @@ 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;
     }
     printf("X:%f Y:%f Z:%f\n",pose->position.x,pose->position.y,pose->position.z);
     printf("W:%f X:%f Y:%f Z:%f\n",pose->orientation.w,pose->orientation.x,pose->orientation.y,pose->orientation.z);
     asyncResult = NodeStatus::RUNNING;
-    connection->get()->planAndExecute(pose,[this](bool finished){
+    connection->planAndExecute(pose,[this](bool finished){
         if(finished){
+            printf("Finished move.");
             asyncResult = NodeStatus::SUCCESS;
         } else {
+            printf("Failed move.");
             asyncResult = NodeStatus::FAILURE;
         }
     });
@@ -40,7 +42,7 @@ BT::NodeStatus BT::RobotMove::onRunning() {
 
 void BT::RobotMove::onHalted() {
     std::cout << "Halting RobotMove" << std::endl;
-    connection->get()->stop();
+    connection->stop();
     asyncResult=NodeStatus::FAILURE;
 }
 

src/btree/src/nodes/RobotMove.h

@@ -16,12 +16,12 @@ namespace BT {
 
     class RobotMove : public StatefulActionNode {
     private:
-        std::shared_ptr<MoveConnection> *connection;
+        std::shared_ptr<MoveConnection> connection;
         NodeStatus asyncResult = NodeStatus::FAILURE;
 
     public:
         RobotMove(const std::string &name, const BT::NodeConfiguration &config,
-                  std::shared_ptr<MoveConnection> *connection);
+                  std::shared_ptr<MoveConnection> connection);
 
         NodeStatus onStart() override;
 

src/btree/src/nodes/SetRobotVelocity.cpp

@@ -11,7 +11,7 @@ BT::NodeStatus BT::SetRobotVelocity::tick() {
         std::cout<<"No velocity given."<<std::endl;
         return NodeStatus::FAILURE;
     }
-    connection->get()->setSpeedMultiplier(velocity);
+    connection->setSpeedMultiplier(velocity);
     return NodeStatus::SUCCESS;
 }
 
@@ -21,7 +21,7 @@ BT::PortsList BT::SetRobotVelocity::providedPorts() {
     };
 }
 
-BT::SetRobotVelocity::SetRobotVelocity(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<MoveConnection> *connection) : SyncActionNode(
+BT::SetRobotVelocity::SetRobotVelocity(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<MoveConnection> connection) : SyncActionNode(
         name, config) {
     this->connection = connection;
 }

src/btree/src/nodes/SetRobotVelocity.h

@@ -11,11 +11,11 @@
 namespace BT {
     class SetRobotVelocity: public SyncActionNode {
     private:
-        std::shared_ptr<MoveConnection> *connection{};
+        std::shared_ptr<MoveConnection> connection;
 
     public:
         SetRobotVelocity(const std::string &name, const NodeConfiguration &config,
-                std::shared_ptr<MoveConnection> *connection);
+                std::shared_ptr<MoveConnection> connection);
 
         static PortsList providedPorts();
 

src/btree/src/nodes/WeightedRandomNode.cpp

@@ -16,7 +16,7 @@ void WeightedRandomNode::halt() {
 
 NodeStatus WeightedRandomNode::tick() {
     if (select_next) {
-        const size_t children_count = children_nodes_.size();
+        size_t children_count = children_nodes_.size();
 
         std::string weightString;
 
@@ -53,14 +53,18 @@ NodeStatus WeightedRandomNode::tick() {
         }
 
         selected_child_index = i;
+        
+        std::cout<< "Selected child:" << selected_child_index <<std::endl;
+        
         select_next = false;
     }
 
     TreeNode *current_child_node = children_nodes_[selected_child_index];
-    const NodeStatus child_status = current_child_node->executeTick();
-
+    NodeStatus child_status = current_child_node->executeTick();
+    
     if (child_status != NodeStatus::RUNNING) {
         select_next = true;
+        resetChildren();
     }
 
     return child_status;

src/btree/trees/actorTreeCoex.xml

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="actorTree">
+    <BehaviorTree ID="actorTree">
+        <WeightedRandom weights="95,5">
+            <Sequence>
+                <SubTree ID="WorkOnBench"/>
+                <SubTree ID="DepositToShelf"/>
+            </Sequence>
+            <Sequence>
+                <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
+                <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="{actorTarget}"/>
+            </Sequence>
+        </WeightedRandom>
+    </BehaviorTree>
+    <BehaviorTree ID="DepositToShelf">
+        <Sequence>
+            <WeightedRandom weights="1,1,1">
+                <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="5.5,0,0,0.707,0,0,-0.707"/>
+                <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="6.5,0,0,0.707,0,0,-0.707"/>
+                <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="7.5,0,0,0.707,0,0,-0.707"/>
+            </WeightedRandom>
+            <WeightedRandom weights="1,1">
+                <Sequence>
+                    <Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
+                    <Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
+                </Sequence>
+                <Sequence>
+                    <Action ID="ActorAnimation" animation_name="standing_to_low"/>
+                    <Action ID="ActorAnimation" animation_name="low_inspect"/>
+                    <Action ID="ActorAnimation" animation_name="low_put_back"/>
+                    <Action ID="ActorAnimation" animation_name="low_to_standing"/>
+                </Sequence>
+            </WeightedRandom>
+        </Sequence>
+    </BehaviorTree>
+    <BehaviorTree ID="WorkOnBench">
+        <Sequence>
+            <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="6.5,6,0,0.707,0,0,0.707"/>
+            <Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
+            <Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
+        </Sequence>
+    </BehaviorTree>
+</root>

src/btree/trees/actorTreeColab.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="actorTree">
+    <BehaviorTree ID="actorTree">
+        <Fallback>
+            <ReactiveSequence>
+                <Inverter>
+                    <Condition ID="IsCalled"/>
+                </Inverter>
+
+                <Sequence>
+                    <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}"/>
+                    </WeightedRandom>
+                    <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="{actorTarget}"/>
+                </Sequence>
+            </ReactiveSequence>
+            <Sequence>
+                <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
+                <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="{actorTarget}"/>
+                <Action ID="ActorAnimation" animation_name="standing_to_low"/>
+                <Action ID="ActorAnimation" animation_name="low_inspect"/>
+                <Action ID="ActorAnimation" animation_name="low_grab"/>
+                <Action ID="ActorAnimation" animation_name="low_to_standing"/>
+                <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="1,0.5,0,0,0,0,1"/>
+                <Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
+                <Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
+                <Action ID="SetCalledTo" state="false"/>
+            </Sequence>
+        </Fallback>
+    </BehaviorTree>
+</root>
+

src/btree/trees/actorTreeCoop.xml

@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="actorTree">
+    <BehaviorTree ID="actorTree">
+        <Sequence>
+            <Control ID="WeightedRandom" weights="95,5">
+                <Sequence>
+                    <SubTree ID="WorkOnBench"/>
+                    <SubTree ID="DepositToShelf"/>
+                </Sequence>
+                <Sequence>
+                    <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
+                    <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="{actorTarget}"/>
+                </Sequence>
+            </Control>
+            <Condition ID="IsCalled"/>
+            <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="1,0.5,0,0,0,0,1"/>
+            <Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
+            <Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
+            <Action ID="SetCalledTo" state="false"/>
+            <SubTree ID="DepositToShelf"/>
+        </Sequence>
+    </BehaviorTree>
+    <BehaviorTree ID="DepositToShelf">
+        <Sequence>
+            <WeightedRandom weights="1,1,1">
+                <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="5.5,0,0,0.707,0,0,-0.707"/>
+                <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="6.5,0,0,0.707,0,0,-0.707"/>
+                <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="7.5,0,0,0.707,0,0,-0.707"/>
+            </WeightedRandom>
+            <WeightedRandom weights="1,1">
+                <Sequence>
+                    <Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
+                    <Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
+                </Sequence>
+                <Sequence>
+                    <Action ID="ActorAnimation" animation_name="standing_to_low"/>
+                    <Action ID="ActorAnimation" animation_name="low_inspect"/>
+                    <Action ID="ActorAnimation" animation_name="low_put_back"/>
+                    <Action ID="ActorAnimation" animation_name="low_to_standing"/>
+                </Sequence>
+            </WeightedRandom>
+        </Sequence>
+    </BehaviorTree>
+    <BehaviorTree ID="WorkOnBench">
+        <Sequence>
+            <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="6.5,6,0,0.707,0,0,0.707"/>
+            <Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
+            <Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
+        </Sequence>
+    </BehaviorTree>
+</root>
+

src/btree/trees/robotTreeCoex.xml

@@ -0,0 +1,23 @@
+<?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}"/>
+                <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>
+        </ReactiveSequence>
+    </BehaviorTree>
+</root>

src/btree/trees/robotTreeColab.xml

@@ -0,0 +1,29 @@
+<?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>
+                <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="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" failure_chance="0.05"/>
+                </InterruptableSequence>
+                <Action ID="SetCalledTo" state="true"/>
+            </Fallback>
+        </ReactiveSequence>
+    </BehaviorTree>
+</root>
+

src/btree/trees/robotTreeCoop.xml

@@ -0,0 +1,33 @@
+<?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>
+</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/btree_trees/trees/actorTree.xml

@@ -1,73 +0,0 @@
-<?xml version="1.0"?>
-<root main_tree_to_execute="BehaviorTree">
-    <!-- ////////// -->
-    <BehaviorTree ID="BehaviorTree">
-        <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="walk" target="{actorTarget}"/>
-                </Sequence>
-            </ReactiveSequence>
-            <Sequence>
-                <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
-                <Action ID="ActorMovement" animation="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/trees/robotTree.xml

@@ -1,73 +0,0 @@
-<?xml version="1.0"?>
-<root main_tree_to_execute="BehaviorTree">
-    <!-- ////////// -->
-    <BehaviorTree ID="BehaviorTree">
-        <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="RandomFailure" failureChance="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"/>
-                </Control>
-                <Action ID="SetCalledTo" state="true"/>
-            </Fallback>
-        </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/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,189 @@
+#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/CMakeLists.txt

@@ -33,7 +33,8 @@ target_link_libraries(ign_actor_plugin
 
 install(TARGETS
     ign_actor_plugin
-    DESTINATION lib/${PROJECT_NAME}
+    DESTINATION share/${PROJECT_NAME}
 )
+install(DIRECTORY   animation    DESTINATION   share/${PROJECT_NAME})
 
 ament_package()

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -68,12 +68,8 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char *animationN
 }
 
 double Angle(ignition::math::Quaterniond a,ignition::math::Quaterniond b){
-    auto dot = fmin(abs(a.Dot(b)),1.0);
-    if(dot > 0.999999){
-        return 0.0;
-    }else{
-        return acos(dot) * 2;
-    }
+    auto dot = fmin(abs(a.Normalized().Dot(b.Normalized())),1.0);
+    return acos(dot);
 }
 
 void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) {
@@ -124,11 +120,12 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
         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 +163,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;
@@ -183,6 +180,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
             }
 
             movementDetails.rotateToEndDuration = Angle(movementDetails.targetDiff.Rot(),movementDetails.target.Rot()) / turnSpeed;
+            
+            igndbg << movementDetails.rotateToTargetDuration << " " <<movementDetails.moveDuration << " " << movementDetails.rotateToEndDuration << std::endl;
         }
         
         movementDetails.time += deltaTimeSeconds;
@@ -190,6 +189,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 +311,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

@@ -1,16 +1,30 @@
 import os
 from ament_index_python import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler, TimerAction
 from launch.event_handlers import OnProcessExit
 from launch_ros.actions import Node, SetParameter
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from moveit_configs_utils import MoveItConfigsBuilder
-import xacro 
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch.conditions import LaunchConfigurationEquals
+import xacro
+
 
 def generate_launch_description():
+    scene_launch_arg = DeclareLaunchArgument(
+        "scene", default_value=TextSubstitution(text="Coex")
+    )
+
+    worldFile = open('/tmp/gazebo_world.sdf', 'w')
+    worldFile.write(xacro.process(os.path.join(get_package_share_directory('ign_world'), 'world', 'world.sdf')))
+    worldFile.flush()
+    worldFile.close()
+
+    scene = LaunchConfiguration('scene')
+
     moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config").to_moveit_configs()
-    
+
     load_joint_state_broadcaster = ExecuteProcess(
         cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'],
         output='screen'
@@ -32,10 +46,71 @@ def generate_launch_description():
         output='screen'
     )
 
+    btree = Node(
+        package='btree',
+        executable='tree',
+        output='both',
+        parameters=[
+            {
+                "trees": [
+                    [get_package_share_directory('btree'), '/trees/actorTree', scene, '.xml'],
+                    [get_package_share_directory('btree'), '/trees/robotTree', scene, '.xml'],
+                ],
+                "areas": [
+                    "safeArea",
+                    "1,   3.5 |" +
+                    "1,   7   |" +
+                    "3,   3.5 |" +
+                    "7,   7   |" +
+                    "3,  -1   |" +
+                    "7,  -1",
+                    "warningArea",
+                    "-1,  2.5 |" +
+                    "-1,  3.5 |" +
+                    "2,   2.5 |" +
+                    "3,   3.5 |" +
+                    "2,  -1   |" +
+                    "3,  -1",
+                    "unsafeArea",
+                    "-1,  1.5 |" +
+                    "-1,  2.5 |" +
+                    "1,   1.5 |" +
+                    "2,   2.5 |" +
+                    "1,  -1   |" +
+                    "2,  -1",
+                    "negativeYTable",
+                    "0.3,  -0.25 |" +
+                    "-0.3, -0.25 |" +
+                    "0,    -0.4",
+                    "positiveYTable",
+                    "0.3,   0.25 |" +
+                    "-0.3,  0.25 |" +
+                    "0,     0.4"
+                ]
+            },
+        ],
+        emulate_tty=True,
+    )
+
     return LaunchDescription([
+        scene_launch_arg,
+
         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(),
+            launch_arguments={'gz_args': '-v 4 -r /tmp/gazebo_world.sdf'}.items(),
+        ),
+
+        Node(
+            package='ros_gz_sim',
+            executable='create',
+            arguments=[
+                '-name', 'conveyor',
+                '-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',
+            condition=LaunchConfigurationEquals('scene', 'Coop')
         ),
 
         Node(
@@ -51,50 +126,13 @@ def generate_launch_description():
             package='ros_actor_action_server',
             executable='ros_actor_action_server',
             output='both',
+            emulate_tty=True,
         ),
         
-        Node(
-            package='btree_nodes',
-            executable='tree',
-            output='both',
-            parameters=[
-                {
-                    "trees": [
-                        get_package_share_directory('btree_trees')+'/trees/actorTree.xml',
-                        get_package_share_directory('btree_trees')+'/trees/robotTree.xml'
-                    ],
-                    "areas": [
-                        "safeArea",
-                        "1,   3.5 |"+
-                        "1,   7   |"+
-                        "3,   3.5 |"+
-                        "7,   7   |"+
-                        "3,  -1   |"+
-                        "7,  -1",
-                        "warningArea",
-                        "-1,  2.5 |"+
-                        "-1,  3.5 |"+
-                        "2,   2.5 |"+
-                        "3,   3.5 |"+
-                        "2,  -1   |"+
-                        "3,  -1",
-                        "unsafeArea",
-                        "-1,  1.5 |"+
-                        "-1,  2.5 |"+
-                        "1,   1.5 |"+
-                        "2,   2.5 |"+
-                        "1,  -1   |"+
-                        "2,  -1",
-                        "negativeYTable",
-                        "0.3,  -0.25 |"+
-                        "-0.3, -0.25 |"+
-                        "0,    -0.4",
-                        "positiveYTable",
-                        "0.3,   0.25 |"+
-                        "-0.3,  0.25 |"+
-                        "0,     0.4"
-                    ]
-                },
+        TimerAction(
+            period=10.0,
+            actions=[
+                btree,
             ]
         ),
 
@@ -116,6 +154,7 @@ def generate_launch_description():
                 on_exit=[load_joint_state_broadcaster],
             )
         ),
+
         RegisterEventHandler(
             event_handler=OnProcessExit(
                 target_action=load_joint_state_broadcaster,
@@ -124,4 +163,4 @@ def generate_launch_description():
         ),
 
         ignition_spawn_entity,
-    ])
+    ])

src/ign_world/launch/moveit_launch.py

@@ -1,311 +0,0 @@
-import os
-import subprocess
-import yaml
-
-from launch import LaunchDescription
-from launch_ros.actions import Node, SetParameter
-from launch.actions import IncludeLaunchDescription, RegisterEventHandler, LogInfo, ExecuteProcess, ExecuteLocal, TimerAction
-from launch_ros.substitutions import FindPackageShare
-from launch.substitutions import PathJoinSubstitution
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from ament_index_python.packages import get_package_share_directory
-from launch.event_handlers import OnProcessExit
-from launch.launch_description_entity import LaunchDescriptionEntity
-from pathlib import Path
-
-
-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:
-            print("Opened file")
-            return file.read()
-    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
-        print("ERROR: load_file " + package_name + " | " + file_path + " -> " + absolute_file_path)
-        return None
-
-
-def load_xacro(package_name, file_path):
-    package_path = get_package_share_directory(package_name)
-    absolute_file_path = os.path.join(package_path, file_path)
-
-    process = subprocess.run(["xacro", absolute_file_path], capture_output=True)
-    if process.returncode != 0:
-        print(process.stderr)
-    return process.stdout.decode("UTF-8")
-
-
-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:
-            print("Opened file")
-            return yaml.safe_load(file)
-    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
-        print("ERROR: load_yaml " + package_name + " | " + file_path + " -> " + absolute_file_path)
-
-        return None
-
-
-def launch_chain(nodes: list[ExecuteLocal]):
-    generatedNodes: list[LaunchDescriptionEntity] = []
-    for index in range(1, len(nodes)):
-        generatedNodes.append(RegisterEventHandler(
-            OnProcessExit(
-                target_action=nodes[index-1],
-                on_exit=nodes[index]
-            )
-        ))
-    generatedNodes.append(nodes[0])
-    return generatedNodes
-
-
-def generate_launch_description():
-
-    robot_description_config = load_xacro('iisy_config', 'urdf/iisy.urdf')
-    robot_description = {'robot_description': robot_description_config}
-
-    robot_description_semantic_config = load_file('iisy_config', 'srdf/iisy.srdf')
-    robot_description_semantic = {'robot_description_semantic': robot_description_semantic_config}
-
-    kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml')
-    robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml}
-
-    controllers = {
-        "moveit_simple_controller_manager": load_yaml("iisy_config", "config/iisy_controllers.yml"),
-        "moveit_controller_manager": "moveit_ros_control_interface/Ros2ControlMultiManager",
-    }
-
-    ompl_planning_pipeline_config = {
-        'move_group': {
-            'planning_plugin': 'ompl_interface/OMPLPlanner',
-            'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
-            'start_state_max_bounds_error': 0.1
-        }
-    }
-    ompl_planning_yaml = load_yaml('iisy_config', 'config/ompl_planning.yml')
-    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
-
-    robot_description_planning = {
-        "robot_description_planning": PathJoinSubstitution(
-            [
-                FindPackageShare("iisy_config"),
-                "config/joint_limits.yml"
-            ]
-        )
-    }
-
-    trajectory_execution = {
-        'moveit_manage_controllers': False,
-        'trajectory_execution.allowed_execution_duration_scaling': 1.2,
-        'trajectory_execution.allowed_goal_duration_margin': 0.5,
-        'trajectory_execution.allowed_start_tolerance': 0.01
-    }
-
-    planning_scene_monitor_parameters = {
-        "publish_planning_scene": True,
-        "publish_geometry_updates": True,
-        "publish_state_updates": True,
-        "publish_transforms_updates": True
-    }
-
-    planning = {
-        "move_group": {
-            "planning_plugin": "ompl_interface/OMPLPlanner",
-            "request_adapters": "default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
-            "start_state_max_bounds_error": 0.1
-        }
-    }
-
-    spawn_robot = Node(
-        package='ros_gz_sim',
-        executable='create',
-        arguments=['-world', 'default', '-string', robot_description_config],
-        output='screen'
-    )
-
-    rviz = Node(
-        package='rviz2',
-        executable='rviz2',
-        arguments=['-d', os.path.join(get_package_share_directory("ign_world"), 'rviz', 'iisy.rviz')],
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            robot_description_planning,
-            ompl_planning_pipeline_config,
-            robot_description_kinematics,
-            planning,
-            trajectory_execution,
-            controllers,
-            planning_scene_monitor_parameters,
-        ],
-    )
-
-    controller_manager = Node(
-            package="controller_manager",
-            executable="ros2_control_node",
-            parameters=[
-                robot_description,
-                str(Path(get_package_share_directory("iisy_config")+"/config/iisy_controllers.yaml")),
-            ],
-    )
-
-    load_joint_state_broadcaster = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'joint_state_broadcaster'],
-        output='both'
-    )
-
-    load_joint_trajectory_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'joint_trajectory_controller'],
-        output='both'
-    )
-
-    move_group = Node(
-        package='moveit_ros_move_group',
-        executable='move_group',
-        # prefix='xterm -fs 10 -e gdb --ex run --args',
-        output='both',
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            robot_description_planning,
-            ompl_planning_pipeline_config,
-            robot_description_kinematics,
-            planning,
-            trajectory_execution,
-            controllers,
-            planning_scene_monitor_parameters,
-            # octomap_config,
-            # octomap_updater_config
-        ]
-    )
-
-    test = Node(
-        package='moveit_test',
-        executable='move',
-        output='both',
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            robot_description_planning,
-            ompl_planning_pipeline_config,
-            robot_description_kinematics,
-            planning,
-            trajectory_execution,
-            controllers,
-            planning_scene_monitor_parameters,
-            # octomap_config,
-            # octomap_updater_config
-        ],
-    )
-
-    # octomap_config = {'octomap_frame': 'camera_rgb_optical_frame',
-    #                  'octomap_resolution': 0.05,
-    #                  'max_range': 5.0}
-
-    # octomap_updater_config = load_yaml('moveit_tutorials', 'config/sensors_3d.yaml')
-
-    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
-
-    return LaunchDescription([
-
-        #IncludeLaunchDescription(
-        #    PythonLaunchDescriptionSource(os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
-        #    launch_arguments={'gz_args': '-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),
-
-        move_group,
-
-        #RegisterEventHandler(
-        #    OnProcessExit(
-        #        target_action=spawn_robot,
-        #        on_exit=[load_joint_state_broadcaster]
-        #    )
-        #),
-
-        controller_manager,
-
-        RegisterEventHandler(
-            OnProcessExit(
-                target_action=load_joint_state_broadcaster,
-                on_exit=[load_joint_trajectory_controller]
-            )
-        ),
-        RegisterEventHandler(
-            OnProcessExit(
-                target_action=load_joint_trajectory_controller,
-                on_exit=TimerAction(period=10.0,actions=[test])
-            )
-        ),
-
-        #spawn_robot,
-        load_joint_state_broadcaster,
-        #load_joint_trajectory_controller,
-        rviz,
-
-        Node(
-            package='ros_actor_action_server',
-            executable='ros_actor_action_server'),
-
-        Node(
-            package='tf2_ros',
-            executable='static_transform_publisher',
-            name='lidar_1_broadcaster',
-            arguments=[
-                "--x", "1",
-                "--y", "-1.9",
-                "--z", "1",
-                "--roll", "1.5707963267949",
-                "--pitch", "0",
-                "--yaw", "0",
-                "--frame-id", "world",
-                "--child-frame-id", "lidar_1_link"]
-        ),
-
-        Node(
-            package='tf2_ros',
-            executable='static_transform_publisher',
-            name='lidar_2_broadcaster',
-            arguments=[
-                "--x", "-1.9",
-                "--y", "1",
-                "--z", "1",
-                "--roll", "0",
-                "--pitch", "0",
-                "--yaw", "0",
-                "--frame-id", "world",
-                "--child-frame-id", "lidar_2_link"]
-        ),
-
-        #Node(
-        #    package='ros_gz_bridge',
-        #    executable='parameter_bridge',
-        #    arguments=["/world/default/model/iisy/joint_state[sensor_msgs/msg/JointState@ignition.msgs.Model"],
-        #    output='both',
-        #    remappings=[
-        #        ('/world/default/model/iisy/joint_state', '/joint_states'),
-        #    ]
-        #),
-
-        Node(
-            package='robot_state_publisher',
-            executable='robot_state_publisher',
-            output='both',
-            parameters=[robot_description]
-        ),
-    ])

src/ign_world/launch/new_launch.py

@@ -1,55 +0,0 @@
-import os
-from ament_index_python import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch_ros.actions import Node, SetParameter
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from moveit_configs_utils import MoveItConfigsBuilder
-from xacro import process as xacro_process
-
-def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config").to_moveit_configs()
-
-    return LaunchDescription([
-        SetParameter(name="use_sim_time", value=True),
-
-        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',
-        ),
-
-        #Node(
-        #    package='ros_gz_bridge',
-        #    executable='parameter_bridge',
-        #    arguments=["/world/default/dynamic_pose/info@tf2_msgs/msg/TFMessage]ignition.msgs.Pose_V"],
-        #    output='both',
-        #    remappings=[
-        #        ('/world/default/dynamic_pose/info', '/tf'),
-        #    ]
-        #),
-
-        Node(
-            package='ros_actor_action_server',
-            executable='ros_actor_action_server'
-        ),
-
-        #Node(
-        #    package='ros_gz_sim',
-        #    executable='create',
-        #    arguments=['-world', 'default', '-string', xacro_process(os.path.join(get_package_share_directory('iisy_config'), 'config', 'iisy_xacro.urdf'))],
-        #    output='screen'
-        #),
-
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                str(moveit_config.package_path / "launch/demo.launch.py")
-            ),
-        ),
-    ])

src/ign_world/world/conveyor.sdf

@@ -0,0 +1,19 @@
+<sdf version="1.6">
+    <model name="conveyor">
+        <static>true</static>
+        <link name="conveyor">
+            <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/ign_world/world/gaz_new_test.sdf

@@ -1,75 +0,0 @@
-<sdf version="1.6">
-  <world name="default">
-    <physics name="1ms" type="ignored">
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1.0</real_time_factor>
-    </physics>
-    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
-    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
-    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
-    <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
-
-    <light type="directional" name="sun">
-      <cast_shadows>true</cast_shadows>
-      <pose>0 0 10 0 0 0</pose>
-      <diffuse>0.8 0.8 0.8 1</diffuse>
-      <specular>0.2 0.2 0.2 1</specular>
-      <attenuation>
-        <range>1000</range>
-        <constant>0.9</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <direction>-0.5 0.1 -0.9</direction>
-    </light>
-    
-    <actor name="actor_walking">
-      <plugin name="ignition::gazebo::ActorSystem" filename="/home/ros/workspace/install/ign_actor_plugin/lib/ign_actor_plugin/libign_actor_plugin.so">
-        <x>2</x>
-        <y>2</y>
-        <rot>180</rot>
-      </plugin>
-      <skin>
-        <filename>/home/ros/walk.dae</filename>
-        <scale>1.0</scale>
-      </skin>
-      <animation name='walk'>
-        <filename>/home/ros/walk.dae</filename>
-        <interpolate_x>true</interpolate_x>
-      </animation>
-      <animation name='standing_extend_arm'>
-        <filename>/home/ros/standing_extend_arm.dae</filename>
-      </animation>
-      <animation name='standing_retract_arm'>
-        <filename>/home/ros/standing_retract_arm.dae</filename>
-      </animation>
-    </actor>
-    
-    <model name="ground_plane">
-      <static>true</static>
-      <link name="link">
-        <collision name="collision">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-        </collision>
-        <visual name="visual">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </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>
-  </world>
-</sdf>

src/ign_world/world/world.sdf

@@ -0,0 +1,119 @@
+<sdf version="1.6">
+    <world name="default">
+        <physics name="1ms" type="ignored">
+            <max_step_size>0.001</max_step_size>
+            <real_time_factor>1.0</real_time_factor>
+        </physics>
+        <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
+        <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
+        <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
+        <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
+    
+        <light type="point" name="point_light_1">
+            <cast_shadows>true</cast_shadows>
+            <pose>1 1 2.5 0 0 0</pose>
+            <visualize>false</visualize>
+        </light>
+        
+        <light type="point" name="point_light_2">
+            <cast_shadows>true</cast_shadows>
+            <pose>5 1 2.5 0 0 0</pose>
+            <visualize>false</visualize>
+        </light>
+        
+        <light type="point" name="point_light_3">
+            <cast_shadows>true</cast_shadows>
+            <pose>1 5 2.5 0 0 0</pose>
+            <visualize>false</visualize>
+        </light>
+        
+        <light type="point" name="point_light_4">
+            <cast_shadows>true</cast_shadows>
+            <pose>5 5 2.5 0 0 0</pose>
+            <visualize>false</visualize>
+        </light>
+        
+        <actor name="actor_walking">
+            <plugin name="ignition::gazebo::ActorSystem" filename="$(find ign_actor_plugin)/libign_actor_plugin.so">
+                <x>2</x>
+                <y>2</y>
+                <rot>180</rot>
+            </plugin>
+            <skin>
+                <filename>$(find ign_actor_plugin)/animation/standing_walk.dae</filename>
+                <scale>1.0</scale>
+            </skin>
+            
+            <animation name='standing_walk'>
+                <filename>$(find ign_actor_plugin)/animation/standing_walk.dae</filename>
+            </animation>
+            <animation name='standing_extend_arm'>
+                <filename>$(find ign_actor_plugin)/animation/standing_extend_arm.dae</filename>
+            </animation>
+            <animation name='standing_retract_arm'>
+                  <filename>$(find ign_actor_plugin)/animation/standing_retract_arm.dae</filename>
+            </animation>
+            <animation name='standing_to_low'>
+                <filename>$(find ign_actor_plugin)/animation/standing_to_low.dae</filename>
+            </animation>
+            
+            <animation name='low_to_standing'>
+                <filename>$(find ign_actor_plugin)/animation/low_to_standing.dae</filename>
+            </animation>
+            <animation name='low_grab'>
+                  <filename>$(find ign_actor_plugin)/animation/low_grab.dae</filename>
+            </animation>
+            <animation name='low_inspect'>
+                <filename>$(find ign_actor_plugin)/animation/low_inspect.dae</filename>
+            </animation>
+            <animation name='low_put_back'>
+                  <filename>$(find ign_actor_plugin)/animation/low_put_back.dae</filename>
+            </animation>
+        </actor>
+        
+        <model name="room">
+            <static>true</static> 
+            <link name="room">
+                <visual name="visual">
+                    <geometry>
+                        <mesh>
+                            <uri>file://$(find ign_world)/model/room.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>
+        
+        <model name="ground_plane">
+            <static>true</static>
+            <link name="link">
+                <collision name="collision">
+                    <geometry>
+                          <plane>
+                              <normal>0 0 1</normal>
+                              <size>100 100</size>
+                          </plane>
+                    </geometry>
+                </collision>
+                <visual name="visual">
+                    <geometry>
+                        <plane>
+                            <normal>0 0 1</normal>
+                            <size>100 100</size>
+                        </plane>
+                    </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>
+    </world>
+</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>
@@ -258,39 +258,76 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
   </transmission>
+  <gazebo reference="table">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <visual>
+      <material>
+        <diffuse>0.8 0.8 0.8 1 </diffuse>
+      </material>
+    </visual>
+  </gazebo>
   <gazebo reference="base_bottom">
     <mu1>0.2</mu1>
     <mu2>0.2</mu2>
-    <material>Gazebo/Orange</material>
+    <visual>
+      <material>
+        <diffuse>0.5 0.5 0.5 1 </diffuse>
+      </material>
+    </visual>
   </gazebo>
   <gazebo reference="base_top">
     <mu1>0.2</mu1>
     <mu2>0.2</mu2>
-    <material>Gazebo/Orange</material>
+    <visual>
+      <material>
+        <diffuse>1 0.35 0 1 </diffuse>
+      </material>
+    </visual>
   </gazebo>
   <gazebo reference="link1_full">
     <mu1>0.2</mu1>
     <mu2>0.2</mu2>
-    <material>Gazebo/Orange</material>
+    <visual>
+      <material>
+        <diffuse>1 0.35 0 1 </diffuse>
+      </material>
+    </visual>
   </gazebo>
   <gazebo reference="link2_bottom">
     <mu1>0.2</mu1>
     <mu2>0.2</mu2>
-    <material>Gazebo/Orange</material>
+    <visual>
+      <material>
+        <diffuse>1 0.35 0 1 </diffuse>
+      </material>
+    </visual>
   </gazebo>
   <gazebo reference="link2_top">
     <mu1>0.2</mu1>
     <mu2>0.2</mu2>
-    <material>Gazebo/Orange</material>
+    <visual>
+      <material>
+        <diffuse>1 0.35 0 1 </diffuse>
+      </material>
+    </visual>
   </gazebo>
   <gazebo reference="link3_bottom">
     <mu1>0.2</mu1>
     <mu2>0.2</mu2>
-    <material>Gazebo/Orange</material>
+    <visual>
+      <material>
+        <diffuse>1 0.35 0 1 </diffuse>
+      </material>
+    </visual>
   </gazebo>
   <gazebo reference="link3_top">
     <mu1>0.2</mu1>
     <mu2>0.2</mu2>
-    <material>Gazebo/Grey</material>
+    <visual>
+      <material>
+        <diffuse>0.5 0.5 0.5 1 </diffuse>
+      </material>
+    </visual>
   </gazebo>
 </robot>

src/iisy_config/config/joint_limits.yaml

@@ -2,39 +2,39 @@
 
 # For beginners, we downscale velocity and acceleration limits.
 # You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
-default_velocity_scaling_factor: 0.1
-default_acceleration_scaling_factor: 0.1
+default_velocity_scaling_factor: 1
+default_acceleration_scaling_factor: 1
 
 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
 # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
 joint_limits:
   base_link1_joint:
     has_velocity_limits: true
-    max_velocity: 1.7453292519943
+    max_velocity: 3.49065850399
     has_acceleration_limits: false
     max_acceleration: 0
   base_rot:
     has_velocity_limits: true
-    max_velocity: 1.7453292519943
+    max_velocity: 3.49065850399
     has_acceleration_limits: false
     max_acceleration: 0
   link1_link2_joint:
     has_velocity_limits: true
-    max_velocity: 1.7453292519943
+    max_velocity: 3.49065850399
     has_acceleration_limits: false
     max_acceleration: 0
   link2_link3_joint:
     has_velocity_limits: true
-    max_velocity: 1.7453292519943
+    max_velocity: 5.23598775598
     has_acceleration_limits: false
     max_acceleration: 0
   link2_rot:
     has_velocity_limits: true
-    max_velocity: 1.7453292519943
+    max_velocity: 5.23598775598
     has_acceleration_limits: false
     max_acceleration: 0
   link3_rot:
     has_velocity_limits: true
-    max_velocity: 1.7453292519943
+    max_velocity: 6.98131700798
     has_acceleration_limits: false
     max_acceleration: 0

src/iisy_config/config/ros2_controllers.yaml

@@ -24,7 +24,7 @@ arm_controller:
     state_interfaces:
       - position
       - velocity
-    open_loop_control: true
+    
     constraints:
       stopped_velocity_tolerance: 0.01
       goal_time: 0.0

src/ros_actor_action_server/src/Server.cpp

@@ -25,7 +25,9 @@ void sendAction(mqd_t queue, ActionMessage *message) {
 
 void waitForState(FeedbackMessage *currentFeedback,ActorPluginState state){
     std::unique_lock lock(feedbackMutex);
-    stateCondition.wait(lock,[&currentFeedback,state]{return currentFeedback->state==state;});
+    stateCondition.wait(lock,[&currentFeedback,state]{
+        return currentFeedback->state==state;
+    });
     
     return;
 }
@@ -99,10 +101,12 @@ int main(int argc, char **argv) {
             }
             if (newFeedback.state==IDLE) {
                 if(currentFeedback.state==ANIMATION && currentAnimationGoalPtr!=nullptr){
+                    std::cout << "Finished Animation. " << std::endl;
                     currentAnimationGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Animation_Result>());
                     currentAnimationGoalPtr = nullptr;
                 }
                 if(currentFeedback.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
+                    std::cout << "Finished Movement. " << std::endl;
                     currentMovementGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Movement_Result>());
                     currentMovementGoalPtr = nullptr;
                 }
@@ -120,13 +124,14 @@ int main(int argc, char **argv) {
             rosMutex.lock();
 
             if (currentFeedback.state == IDLE) {
-                std::cout << "Accepting..." << std::endl;
+                std::cout << "Accepting Animation..." << std::endl;
                 
+                currentAction.animationSpeed = animationGoal->animation_speed;
                 strcpy(currentAction.animationName, animationGoal->animation_name.data());
 
                 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
             } else {
-                std::cout << "Rejecting..." << std::endl;
+                std::cout << "Rejecting Animation..." << std::endl;
                 rosMutex.unlock();
                 return rclcpp_action::GoalResponse::REJECT;
             }
@@ -136,9 +141,11 @@ int main(int argc, char **argv) {
                 return rclcpp_action::CancelResponse::REJECT;
             } else {
                 rosMutex.lock();
+                std::cout << "Cancelling Animation..." << std::endl;
                 sendAction(actionQueue, &cancelAction);
                 
                 waitForState(&currentFeedback, IDLE);
+                std::cout << "Cancelled Animation..." << std::endl;
                 rosMutex.unlock();
 
                 return rclcpp_action::CancelResponse::ACCEPT;
@@ -160,15 +167,15 @@ int main(int argc, char **argv) {
         [&currentAction, &currentFeedback](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal) {
             rosMutex.lock();
             if (currentFeedback.state == IDLE) {
-                std::cout << "Accepting..." << std::endl;
+                std::cout << "Accepting Movement..." << 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;
             } else {
-                std::cout << "Rejecting..." << std::endl;
+                std::cout << "Rejecting Movement..." << std::endl;
                 rosMutex.unlock();
                 return rclcpp_action::GoalResponse::REJECT;
             }
@@ -178,9 +185,11 @@ int main(int argc, char **argv) {
                 return rclcpp_action::CancelResponse::REJECT;
             } else {
                 rosMutex.lock();
+                std::cout << "Cancelling Movement..." << std::endl;
                 sendAction(actionQueue, &cancelAction);
                 
                 waitForState(&currentFeedback, IDLE);
+                std::cout << "Cancelled Movement..." << std::endl;
                 rosMutex.unlock();
                 
                 return rclcpp_action::CancelResponse::ACCEPT;

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,19 +1,21 @@
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
 namespace ros_actor_message_queue_msgs{
+  
 enum ActorPluginState{
-  SETUP,IDLE,ANIMATION,MOVEMENT
-  };
+    SETUP,IDLE,ANIMATION,MOVEMENT
+};
 
 struct FeedbackMessage{
-  ActorPluginState state;
-  float progress;
-  geometry_msgs::msg::Pose current;
+    ActorPluginState state;
+    float progress;
+    geometry_msgs::msg::Pose current;
 };
 
 struct ActionMessage{
-  ActorPluginState state;
-  geometry_msgs::msg::Pose target;
-  float animationSpeed = 1.0f,animationDistance = 1.0f;
-  char animationName[256];
+    ActorPluginState state;
+    geometry_msgs::msg::Pose target;
+    float animationSpeed = 1.0f;
+    char animationName[256];
 };
+
 }

test.sh

@@ -1,2 +1,2 @@
-ros2 action send_goal /ActorPlugin/animation ign_actor_plugin_msgs/action/Animation "{animation_name: 'walk', animation_speed: 1.0}"
-ros2 action send_goal /ActorPlugin/movement ros_actor_action_server_msgs/action/Movement '{animation_name: "walk", animation_speed: 1.0, animation_distance: 1.5, target: {orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}, position: {x: 2.0, y: 0.0, z: 0.0}}}'
+ros2 action send_goal /ActorPlugin/animation ign_actor_plugin_msgs/action/Animation "{animation_name: 'standing_walk', animation_speed: 1.0}"
+ros2 action send_goal /ActorPlugin/movement ros_actor_action_server_msgs/action/Movement '{animation_name: "standing_walk", animation_speed: 1.0, animation_distance: 1.5, target: {orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}, position: {x: 2.0, y: 0.0, z: 0.0}}}'