diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt
index 6469ed5..c3ffa0e 100644
--- a/src/btree_nodes/CMakeLists.txt
+++ b/src/btree_nodes/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION VERSION 3.5.1)
 project(btree_nodes)
 
-set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD 20)
 add_compile_options(-Wall -Wextra -Wpedantic)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
@@ -11,13 +11,15 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(behaviortree_cpp_v3 REQUIRED)
+
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-add_executable(tree src/Tree.cpp src/Extensions.cpp)
-ament_target_dependencies(tree geometry_msgs behaviortree_cpp_v3 rclcpp)
+add_executable(tree src/Tree.cpp src/Extensions.cpp src/WeightedRandomNode.cpp)
+ament_target_dependencies(tree rclcpp geometry_msgs std_msgs behaviortree_cpp_v3)
 
 #add_executable(talker src/publisher_member_function.cpp)
 #ament_target_dependencies(talker geometry_msgs rclcpp)
diff --git a/src/btree_nodes/nodes.xml b/src/btree_nodes/nodes.xml
index 95c5c53..2b3fdd5 100644
--- a/src/btree_nodes/nodes.xml
+++ b/src/btree_nodes/nodes.xml
@@ -7,16 +7,28 @@
 <root>
     <TreeNodesModel>
         <!-- ############################### ACTION NODES ################################# -->
-        <Action ID="GenerateNewPose">
-            <input_port name="target_area">Target area</input_port>
-            <output_port name="pose">Generated pose in target area</output_port>
+        <Action ID="AmICalled">
+            <input_port name="called">called status</input_port>
         </Action>
 
-        <Action ID="MoveToPose">
-            <input_port name="pose_publisher">Where to publish target pose</input_port>
-            <input_port name="pose_listener">Where to get current pose</input_port>
-            <input_port name="target_pose">Actual target pose</input_port>
+        <Action ID="GenerateSafeTarget">
+            <output_port name="target">Generated pose in safe area</output_port>
         </Action>
+        <Action ID="GenerateWarningTarget">
+            <output_port name="target">Generated pose in warning area</output_port>
+        </Action>
+        <Action ID="GenerateUnsafeTarget">
+            <output_port name="target">Generated pose in unsafe area</output_port>
+        </Action>
+
+        <Action ID="MoveActorToTarget">
+            <input_port name="current">Current actor position</input_port>
+            <input_port name="target">Target to move actor to</input_port>
+        </Action>
+
+        <Control ID="WeightedRandom">
+            <input_port name="weights">Weights for the children, separated by semicolon.</input_port>
+        </Control>
 
         <Condition ID="InTest">
 
diff --git a/src/btree_nodes/src/MinimalSubscriber.cpp b/src/btree_nodes/src/MinimalSubscriber.cpp
index 6042adf..16892b9 100644
--- a/src/btree_nodes/src/MinimalSubscriber.cpp
+++ b/src/btree_nodes/src/MinimalSubscriber.cpp
@@ -9,7 +9,7 @@
 template<typename T>
 class MinimalSubscriber : public rclcpp::Node {
 public:
-    MinimalSubscriber(const std::string &node_name, const std::string &topic_name, void(*callback)(T)) :
+    MinimalSubscriber(const std::string &node_name, const std::string &topic_name, std::function<void(T)> callback) :
             Node(node_name) {
         this->subscription_ = this->create_subscription<T>(
                 topic_name, 10, [callback](const T result) {
@@ -22,9 +22,10 @@ private:
 };
 
 template<typename T>
-std::thread spinMinimalSubscriber(const std::string &node_name, const std::string &topic_name, void(*callback)(T)) {
-    return std::thread([node_name, topic_name, callback]() {
-        rclcpp::spin(std::make_shared<MinimalSubscriber<T>>(node_name, topic_name, callback));
+std::thread spinMinimalSubscriber(const std::string &node_name, const std::string &topic_name, std::function<void(T)> callback) {
+    auto subscriber = std::make_shared<MinimalSubscriber<T>>(node_name, topic_name, callback);
+    return std::thread([subscriber]() {
+        rclcpp::spin(subscriber);
         rclcpp::shutdown();
     });
 }
\ No newline at end of file
diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp
index 42dc868..12ea005 100644
--- a/src/btree_nodes/src/Tree.cpp
+++ b/src/btree_nodes/src/Tree.cpp
@@ -3,7 +3,9 @@
 #include <behaviortree_cpp_v3/action_node.h>
 #include <rclcpp/rclcpp.hpp>
 #include <random>
+#include <std_msgs/msg/bool.hpp>
 #include "MinimalSubscriber.cpp"
+#include "WeightedRandomNode.h"
 
 //-------------------------------------------------------------
 // Simple Action to print a number
@@ -11,73 +13,96 @@
 
 using namespace BT;
 
+class AmICalled: public ConditionNode
+{
+public:
+    AmICalled(const std::string& name, const NodeConfiguration& config):
+            ConditionNode(name,config)
+    {}
+
+    static PortsList providedPorts()
+    {
+        return { InputPort<bool>("called") };
+    }
+
+    NodeStatus tick() override
+    {
+        bool called;
+        if(!getInput("called",called)){
+            return NodeStatus::FAILURE;
+        }
+        if(called){
+            return NodeStatus::SUCCESS;
+        }
+        return NodeStatus::FAILURE;
+    }
+};
+
 typedef std::chrono::milliseconds Milliseconds;
 
-class MyAsyncAction : public CoroActionNode {
+class MoveActorToTarget : public StatefulActionNode {
 public:
-    MyAsyncAction(const std::string &name) :
-            CoroActionNode(name, {}) {}
+    MoveActorToTarget(const std::string &name, const NodeConfiguration &config)
+            : StatefulActionNode(name, config) {}
 
-private:
-    // This is the ideal skeleton/template of an async action:
-    //  - A request to a remote service provider.
-    //  - A loop where we check if the reply has been received.
-    //  - You may call setStatusRunningAndYield() to "pause".
-    //  - Code to execute after the reply.
-    //  - A simple way to handle halt().
-    NodeStatus tick() override {
-        std::cout << name() << ": Started. Send Request to server." << std::endl;
-
-        TimePoint initial_time = Now();
-        TimePoint time_before_reply = initial_time + Milliseconds(100);
-
-        int count = 0;
-        bool reply_received = false;
-
-        while (!reply_received) {
-            if (count++ == 0) {
-                // call this only once
-                std::cout << name() << ": Waiting Reply..." << std::endl;
-            }
-            // pretend that we received a reply
-            if (Now() >= time_before_reply) {
-                reply_received = true;
-            }
-
-            if (!reply_received) {
-                // set status to RUNNING and "pause/sleep"
-                // If halt() is called, we will NOT resume execution
-                setStatusRunningAndYield();
-            }
-        }
-
-        // This part of the code is never reached if halt() is invoked,
-        // only if reply_received == true;
-        std::cout << name() << ": Done. 'Waiting Reply' loop repeated "
-                  << count << " times" << std::endl;
-        cleanup(false);
-        return NodeStatus::SUCCESS;
+    static PortsList providedPorts() {
+        return {
+                InputPort<geometry_msgs::msg::Pose>("current"),
+                InputPort<geometry_msgs::msg::Pose>("target")
+        };
     }
 
-    // you might want to cleanup differently if it was halted or successful
-    void cleanup(bool halted) {
-        if (halted) {
-            std::cout << name() << ": cleaning up after an halt()\n" << std::endl;
+    geometry_msgs::msg::Pose target;
+
+
+    NodeStatus onStart() override {
+        std::cout << "started moving" << std::endl;
+
+        rclcpp::Node node("targetPublisherNode");
+        auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
+
+        auto res = getInput<geometry_msgs::msg::Pose>("target", target);
+        if (!res) {
+            std::cout << "[ no target available ]" << std::endl;
+            std::cout << res.error() << std::endl;
+            return NodeStatus::FAILURE;
+        }
+
+        publisher->publish(target);
+        return NodeStatus::RUNNING;
+    }
+
+    NodeStatus onRunning() override {
+        geometry_msgs::msg::Pose current;
+
+        auto res = getInput<geometry_msgs::msg::Pose>("current", current);
+        if (!res) {
+            std::cout << "[ no current position available ]" << std::endl;
+            std::cout << res.error() << std::endl;
+            return NodeStatus::FAILURE;
+        }
+
+        double dX = target.position.x - current.position.x;
+        double dY = target.position.y - current.position.y;
+
+        auto distance = sqrt(dX * dX + dY * dY);
+
+        if (distance < 0.3) {
+            return NodeStatus::SUCCESS;
         } else {
-            std::cout << name() << ": cleaning up after SUCCESS\n" << std::endl;
+            return NodeStatus::RUNNING;
         }
     }
 
-    void halt() override {
-        std::cout << name() << ": Halted." << std::endl;
-        cleanup(true);
-        // Do not forget to call this at the end.
-        CoroActionNode::halt();
+    void onHalted() override {
+        std::cout << "halted move" << std::endl;
+        rclcpp::Node node("targetPublisherNode");
+        auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
+        geometry_msgs::msg::Pose inf;
+        inf.position.x = HUGE_VAL;
+        inf.position.y = HUGE_VAL;
+        publisher->publish(inf);
     }
-
-    TimePoint Now() {
-        return std::chrono::high_resolution_clock::now();
-    };
 };
 
 // This class cannot guarantee valid results with every polygon, triangles are assumed to be a valid triangle-strip.
@@ -153,7 +178,7 @@ public:
     static PortsList providedPorts() {
         // Optionally, a port can have a human readable description
         const char *description = "Generates a random position in an area defined by a triangle strip";
-        return {OutputPort<Position2D>("target", description)};
+        return {OutputPort<geometry_msgs::msg::Pose>("target", description)};
     }
 
 
@@ -163,57 +188,88 @@ public:
         while (triangleRandom < weights[i + 1] && i < weights.size() - 1) {
             i++;
         }
-        Position2D randomPos = getRandomPosInTriangle(i);
+        auto pos = getRandomPosInTriangle(i);
 
-        setOutput<Position2D>("targetPosition", randomPos);
+        geometry_msgs::msg::Pose randomPose;
+        randomPose.position.x=pos.x;
+        randomPose.position.y=pos.y;
+
+        printf("Generated Target: %f %f\n",pos.x,pos.y);
+        setOutput<geometry_msgs::msg::Pose>("target", randomPose);
         return NodeStatus::SUCCESS;
     }
 };
 
 int main(int argc, char **argv) {
+    std::cout << "Registering nodes." << std::endl;
     NodeBuilder builderGenerateSafeTarget = [](const std::string &name, const NodeConfiguration &config) {
-        const Area areaSafe = {std::vector<Position2D>({{1, 1},
+        const Area area = {std::vector<Position2D>({{1, 1},
+                                                        {5, 1},
+                                                        {5, 5},
+                                                        {1, 5}})};
+        return std::make_unique<GenerateTarget2D>(name, config, area);
+    };
+    NodeBuilder builderGenerateWarningTarget = [](const std::string &name, const NodeConfiguration &config) {
+        const Area area = {std::vector<Position2D>({{1, 1},
                                                         {2, 2},
                                                         {1, 2}})};
-        return std::make_unique<GenerateTarget2D>(name, config, areaSafe);
+        return std::make_unique<GenerateTarget2D>(name, config, area);
+    };
+    NodeBuilder builderGenerateUnsafeTarget = [](const std::string &name, const NodeConfiguration &config) {
+        const Area area = {std::vector<Position2D>({{1, 1},
+                                                        {2, 2},
+                                                        {1, 2}})};
+        return std::make_unique<GenerateTarget2D>(name, config, area);
     };
 
     rclcpp::init(argc, argv);
 
-    new Area();
-
     BehaviorTreeFactory factory;
     factory.registerBuilder<GenerateTarget2D>("GenerateSafeTarget", builderGenerateSafeTarget);
-    factory.registerBuilder<GenerateTarget2D>("GenerateWarningTarget", builderGenerateSafeTarget);
-    factory.registerBuilder<GenerateTarget2D>("GenerateUnsafeTarget", builderGenerateSafeTarget);
+    factory.registerBuilder<GenerateTarget2D>("GenerateWarningTarget", builderGenerateWarningTarget);
+    factory.registerBuilder<GenerateTarget2D>("GenerateUnsafeTarget", builderGenerateUnsafeTarget);
 
-    Tree tree = factory.createTreeFromFile("actor_tree.xml");
+    factory.registerNodeType<AmICalled>("AmICalled");
+
+    factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
+    factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
+
+    std::cout << "Creating tree." << std::endl;
+
+    Tree tree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/tree.xml");
+
+    std::mutex mutex;
+
+    std::cout << "Starting subscriber." << std::endl;
+
+    geometry_msgs::msg::Pose init;
+    tree.rootBlackboard()->set<geometry_msgs::msg::Pose>("current",init);
+    tree.rootBlackboard()->set<geometry_msgs::msg::Pose>("target",init);
+    tree.rootBlackboard()->set<bool>("called",false);
+
+    spinMinimalSubscriber<geometry_msgs::msg::Pose>("tree_get_currentPose", "currentPose",
+                                                    [&tree, &mutex](geometry_msgs::msg::Pose pose) {
+                                                        mutex.lock();
+                                                        tree.rootBlackboard()->set("current", pose);
+                                                        mutex.unlock();
+                                                    }).detach();
+
+    spinMinimalSubscriber<std_msgs::msg::Bool>("tree_get_called", "called",
+                                                    [&tree, &mutex](std_msgs::msg::Bool called) {
+                                                        mutex.lock();
+                                                        tree.rootBlackboard()->set("called", (bool) called.data);
+                                                        mutex.unlock();
+                                                    }).detach();
+
+    std::cout << "Looping." << std::endl;
 
     while (rclcpp::ok()) {
+        mutex.lock();
         tree.tickRoot();
+        mutex.unlock();
     }
-    /*
-    NodeHandle nh;
 
-    BehaviorTreeFactory factory;
-
-    //factory.registerNodeType<PrintValue>("PrintValue");
-    //RegisterRosService<AddTwoIntsAction>(factory, "AddTwoInts", nh);
-    //RegisterRosAction<FibonacciServer>(factory, "Fibonacci", nh);
-
-    auto tree = factory.createTreeFromText("");
-
-    NodeStatus status = NodeStatus::IDLE;
-
-    while( rclcpp::ok() && (status == NodeStatus::IDLE || status == NodeStatus::RUNNING))
-    {
-        rclcpp::spinOnce();
-        status = tree.tickRoot();
-        std::cout << status << std::endl;
-        ros::Duration sleep_time(0.01);
-        sleep_time.sleep();
-    }
-     */
+    std::cout << "End." << std::endl;
 
     return 0;
 }
\ No newline at end of file
diff --git a/src/btree_nodes/src/WeightedRandomNode.cpp b/src/btree_nodes/src/WeightedRandomNode.cpp
new file mode 100644
index 0000000..513a38e
--- /dev/null
+++ b/src/btree_nodes/src/WeightedRandomNode.cpp
@@ -0,0 +1,70 @@
+//
+// Created by bastian on 08.03.22.
+//
+
+#include "WeightedRandomNode.h"
+#include <regex>
+
+WeightedRandomNode::WeightedRandomNode(const std::string &name, const NodeConfiguration &config)
+        : ControlNode::ControlNode(name, config), selected_child_index(0), select_next(true) {
+    setRegistrationID("Sequence");
+}
+
+void WeightedRandomNode::halt() {
+    select_next = true;
+    ControlNode::halt();
+}
+
+NodeStatus WeightedRandomNode::tick() {
+    if (select_next) {
+        const size_t children_count = children_nodes_.size();
+
+        std::string weightString;
+
+        if(!getInput<std::string>("weights",weightString)){
+            std::cout<< "Weights are REQUIRED." <<std::endl;
+            return NodeStatus::FAILURE;
+        }
+
+        std::regex rgx(";");
+        std::sregex_token_iterator iter(weightString.begin(),
+                                        weightString.end(),
+                                        rgx,
+                                        -1);
+        std::sregex_token_iterator end;
+        std::vector<double> weights;
+        double sum = 0;
+        unsigned long i=0;
+        for (; iter != end; ++iter) {
+            sum+=stod(*iter);
+            weights.push_back(sum);
+            i++;
+        }
+
+        if(weights.size()!=children_count){
+            std::cout<< "Number of weights doesn't match child count." <<std::endl;
+            return NodeStatus::FAILURE;
+        }
+
+        std::uniform_real_distribution<double> randomRange(0, sum);
+        double random = randomRange(rng);
+
+        i=0;
+        while (i<weights.size() && random>weights[i]){
+            i++;
+        }
+
+        selected_child_index = i;
+        select_next = false;
+    }
+
+    TreeNode *current_child_node = children_nodes_[selected_child_index];
+    const NodeStatus child_status = current_child_node->executeTick();
+
+    if (child_status != NodeStatus::RUNNING) {
+        select_next = true;
+        printf("node %zu finished.\n",selected_child_index);
+    }
+
+    return child_status;
+}
\ No newline at end of file
diff --git a/src/btree_nodes/src/WeightedRandomNode.h b/src/btree_nodes/src/WeightedRandomNode.h
new file mode 100644
index 0000000..a2f704b
--- /dev/null
+++ b/src/btree_nodes/src/WeightedRandomNode.h
@@ -0,0 +1,35 @@
+//
+// Created by bastian on 08.03.22.
+//
+
+#ifndef BUILD_WEIGHTEDRANDOMNODE_H
+#define BUILD_WEIGHTEDRANDOMNODE_H
+
+#include <behaviortree_cpp_v3/control_node.h>
+#include <string>
+#include <random>
+
+using namespace BT;
+
+class WeightedRandomNode : public ControlNode
+{
+public:
+    WeightedRandomNode(const std::string& name, const NodeConfiguration &config);
+
+    virtual ~WeightedRandomNode() override = default;
+
+    virtual void halt() override;
+
+    static PortsList providedPorts(){
+        return { InputPort<bool>("weights") };
+    }
+private:
+    size_t selected_child_index;
+    bool select_next{};
+    std::mt19937_64 rng;
+
+    virtual BT::NodeStatus tick() override;
+};
+
+
+#endif //BUILD_WEIGHTEDRANDOMNODE_H
diff --git a/src/btree_nodes/tree.xml b/src/btree_nodes/tree.xml
new file mode 100644
index 0000000..18e3ded
--- /dev/null
+++ b/src/btree_nodes/tree.xml
@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <ReactiveSequence>
+            <Action ID="AmICalled" called="{called}"/>
+            <Sequence>
+                <Control ID="WeightedRandom" weights="10;2;1">
+                    <Action ID="GenerateSafeTarget" target="{target}"/>
+                    <Action ID="GenerateWarningTarget" target="{target}"/>
+                    <Action ID="GenerateUnsafeTarget" target="{target}"/>
+                </Control>
+                <Action ID="MoveActorToTarget" current="{current}" target="{target}"/>
+            </Sequence>
+        </ReactiveSequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="AmICalled">
+            <input_port name="called">called status</input_port>
+        </Action>
+        <Action ID="GenerateSafeTarget">
+            <output_port name="target">Generated pose in safe area</output_port>
+        </Action>
+        <Action ID="GenerateUnsafeTarget">
+            <output_port name="target">Generated pose in unsafe area</output_port>
+        </Action>
+        <Action ID="GenerateWarningTarget">
+            <output_port name="target">Generated pose in warning area</output_port>
+        </Action>
+        <Condition ID="InTest"/>
+        <Action ID="MoveActorToTarget">
+            <input_port name="current">Current actor position</input_port>
+            <input_port name="target">Target to move actor to</input_port>
+        </Action>
+        <Control ID="WeightedRandom">
+            <input_port name="weights">Weights for the children, separated by semicolon.</input_port>
+        </Control>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
+
diff --git a/src/cpp_pubsub/src/publisher_member_function.cpp b/src/cpp_pubsub/src/publisher_member_function.cpp
index 5d6d2ba..2b46cb8 100644
--- a/src/cpp_pubsub/src/publisher_member_function.cpp
+++ b/src/cpp_pubsub/src/publisher_member_function.cpp
@@ -27,7 +27,7 @@ using namespace std::chrono_literals;
 class MinimalPublisher : public rclcpp::Node{
     public:
         MinimalPublisher(): Node("minimal_publisher") {
-            publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("topic", 10);
+            publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
             timer_ = this->create_wall_timer(
                 500ms, [this]{
                     auto message = geometry_msgs::msg::Pose();
diff --git a/src/gaz_simulation/CMakeLists.txt b/src/gaz_simulation/CMakeLists.txt
index 7e51433..58a02eb 100644
--- a/src/gaz_simulation/CMakeLists.txt
+++ b/src/gaz_simulation/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED)
 install(DIRECTORY   launch    DESTINATION   share/${PROJECT_NAME})
 install(DIRECTORY   world     DESTINATION   share/${PROJECT_NAME})
 install(DIRECTORY   urdf      DESTINATION   share/${PROJECT_NAME})
+install(DIRECTORY   stl       DESTINATION   share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
diff --git a/src/gaz_simulation/launch/test_launch.py b/src/gaz_simulation/launch/test_launch.py
index faac12b..9858884 100644
--- a/src/gaz_simulation/launch/test_launch.py
+++ b/src/gaz_simulation/launch/test_launch.py
@@ -14,11 +14,19 @@ def generate_launch_description():
     world_path = os.path.join(path, "world/test2.xml")
     urdf_path = os.path.join(path, "urdf/iisy.urdf")
     return LaunchDescription([
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
-            launch_arguments={'world': world_path}.items()
+        #IncludeLaunchDescription(
+        #    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
+        #    launch_arguments={'world': world_path}.items()
+        #),
+        
+        ExecuteProcess(
+            cmd=[[
+                'LIBGL_ALWAYS_SOFTWARE=true ros2 launch gazebo_ros gzserver.launch.py "world:='+world_path+'"'
+            ]],
+            shell=True
         ),
 
+
         # Start Gazebo client
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
diff --git a/src/gaz_simulation/stl/base_bottom_coll.stl b/src/gaz_simulation/stl/base_bottom_coll.stl
new file mode 100755
index 0000000..024b506
Binary files /dev/null and b/src/gaz_simulation/stl/base_bottom_coll.stl differ
diff --git a/src/gaz_simulation/stl/base_bottom_low.stl b/src/gaz_simulation/stl/base_bottom_low.stl
new file mode 100755
index 0000000..6bef387
Binary files /dev/null and b/src/gaz_simulation/stl/base_bottom_low.stl differ
diff --git a/src/gaz_simulation/stl/base_top_low.stl b/src/gaz_simulation/stl/base_top_low.stl
new file mode 100755
index 0000000..456a74d
Binary files /dev/null and b/src/gaz_simulation/stl/base_top_low.stl differ
diff --git a/src/gaz_simulation/stl/link_1_full_coll.stl b/src/gaz_simulation/stl/link_1_full_coll.stl
new file mode 100755
index 0000000..4abd360
Binary files /dev/null and b/src/gaz_simulation/stl/link_1_full_coll.stl differ
diff --git a/src/gaz_simulation/stl/link_1_full_low.stl b/src/gaz_simulation/stl/link_1_full_low.stl
new file mode 100755
index 0000000..2ddbeb5
Binary files /dev/null and b/src/gaz_simulation/stl/link_1_full_low.stl differ
diff --git a/src/gaz_simulation/stl/link_2_bottom_low.stl b/src/gaz_simulation/stl/link_2_bottom_low.stl
new file mode 100755
index 0000000..94af640
Binary files /dev/null and b/src/gaz_simulation/stl/link_2_bottom_low.stl differ
diff --git a/src/gaz_simulation/stl/link_2_top_coll.stl b/src/gaz_simulation/stl/link_2_top_coll.stl
new file mode 100755
index 0000000..774fa1c
Binary files /dev/null and b/src/gaz_simulation/stl/link_2_top_coll.stl differ
diff --git a/src/gaz_simulation/stl/link_2_top_low.stl b/src/gaz_simulation/stl/link_2_top_low.stl
new file mode 100755
index 0000000..42a9a10
Binary files /dev/null and b/src/gaz_simulation/stl/link_2_top_low.stl differ
diff --git a/src/gaz_simulation/stl/link_3_bottom_coll.stl b/src/gaz_simulation/stl/link_3_bottom_coll.stl
new file mode 100755
index 0000000..e51ba52
Binary files /dev/null and b/src/gaz_simulation/stl/link_3_bottom_coll.stl differ
diff --git a/src/gaz_simulation/stl/link_3_bottom_low.stl b/src/gaz_simulation/stl/link_3_bottom_low.stl
new file mode 100755
index 0000000..5f1de26
Binary files /dev/null and b/src/gaz_simulation/stl/link_3_bottom_low.stl differ
diff --git a/src/gaz_simulation/stl/link_3_top_coll.stl b/src/gaz_simulation/stl/link_3_top_coll.stl
new file mode 100755
index 0000000..48214ea
Binary files /dev/null and b/src/gaz_simulation/stl/link_3_top_coll.stl differ
diff --git a/src/gaz_simulation/stl/link_3_top_low.stl b/src/gaz_simulation/stl/link_3_top_low.stl
new file mode 100755
index 0000000..615b5e2
Binary files /dev/null and b/src/gaz_simulation/stl/link_3_top_low.stl differ
diff --git a/src/gaz_simulation/urdf/iisy.urdf b/src/gaz_simulation/urdf/iisy.urdf
index 4bca97e..ea77848 100644
--- a/src/gaz_simulation/urdf/iisy.urdf
+++ b/src/gaz_simulation/urdf/iisy.urdf
@@ -1,195 +1,263 @@
-<?xml version="1.0"?>
-<robot name="iisy" xmlns:xacro="http://ros.org/wiki/xacro">
-        <static>true</static>
-        <link name='base'>
-            <pose>0.025 0 0.05 0 0 0</pose>
-            <collision name='collision'>
-                <geometry>
-                    <box size=".15 .1 .1"/>
-                </geometry>
-            </collision>
-            <visual name='visual'>
-                <geometry>
-                    <box size=".15 .1 .1"/>
-                </geometry>
-            </visual>
-            <inertial>
-                <mass value="6"/>
-                <inertia ixx="0.0032666666666667" ixy="0.0" ixz="0.0" iyy="0.0032666666666667" iyz="0.0" izz="0.0025"/>
-            </inertial>
-        </link>
-        
-        <link name="base_rot">
-            <pose>0 0 0.155 0 0 0</pose>
-            <collision name="collision">
-                <geometry>
-                    <cylinder radius=".05" length=".11"/>
-                </geometry>
-            </collision>
-            <visual name="visual">
-                <geometry>
-                    <cylinder radius=".05" length=".11"/>
-                </geometry>
-            </visual>
-            <inertial>
-                <mass value="2"/>
-                <inertia ixx="0.0032666666666667" ixy="0.0" ixz="0.0" iyy="0.0032666666666667" iyz="0.0" izz="0.0025"/>
-            </inertial>
-        </link>
-        
-        <link name="link1">
-            <pose>0 0 0.36 0 0 0</pose>
-            <collision name="collision">
-                <geometry>
-                    <cylinder radius=".05" length=".30"/>
-                </geometry>
-            </collision>
-            <visual name="visual">
-                <geometry>
-                    <cylinder radius=".05" length=".30"/>
-                </geometry>
-            </visual>
-            <inertial>
-                <mass value="4"/>
-                <inertia ixx="0.0325" ixy="0.0" ixz="0.0" iyy="0.0325" iyz="0.0" izz="0.005"/>
-            </inertial>
-        </link>
-        
-        <link name="link2_rot">
-            <pose>0 0 0.585 0 0 0</pose>
-            <collision name="collision">
-                <geometry>
-                    <cylinder radius=".05" length=".15"/>
-                </geometry>
-            </collision>
-            <visual name="visual">
-                <geometry>
-                    <cylinder radius=".05" length=".15"/>
-                </geometry>
-            </visual>
-            <inertial>
-                <mass value="2"/>
-                <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.0025"/>
-            </inertial>
-        </link>
-        
-        <link name="link2">
-            <pose>0 0 0.735 0 0 0</pose>
-            <collision name="collision">
-                <geometry>
-                    <cylinder radius=".05" length=".15"/>
-                </geometry>
-            </collision>
-            <visual name="visual">
-                <geometry>
-                    <cylinder radius=".05" length=".15"/>
-                </geometry>
-            </visual>
-            <inertial>
-                <mass value="2"/>
-                <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.0025"/>
-            </inertial>
-        </link>
-        
-        <link name="link3_rot">
-            <pose>0 0 0.86 0 0 0</pose>
-            <collision name="collision">
-                <geometry>
-                    <cylinder radius=".05" length=".10"/>
-                </geometry>
-            </collision>
-            <visual name="visual">
-                <geometry>
-                    <cylinder radius=".05" length=".10"/>
-                </geometry>
-            </visual>
-            <inertial>
-                <mass value="2"/>
-                <inertia ixx="0.0029166666666667" ixy="0.0" ixz="0.0" iyy="0.0029166666666667" iyz="0.0" izz="0.0025"/>
-            </inertial>
-        </link>
-        
-        <link name="link3">
-            <pose>0 0 0.94025 0 0 0</pose>
-            <collision name="collision">
-                <geometry>
-                    <cylinder radius=".05" length=".0605"/>
-                </geometry>
-            </collision>
-            <visual name="visual">
-                <geometry>
-                    <cylinder radius=".05" length=".0605"/>
-                </geometry>
-            </visual>
-            <inertial>
-                <mass value="0.8"/>
-                <inertia ixx="0.00074401666666667" ixy="0.0" ixz="0.0" iyy="0.00074401666666667" iyz="0.0" izz="0.001"/>
-            </inertial>
-        </link>
-        
-        <joint type="continuous" name="base_rot_joint">
-            <pose>0 0 -0.055 0 0 0</pose>
-            <parent link="base"/>
-            <child link="base_rot"/>
-            <axis>
-                <xyz>0 0 1</xyz>
-            </axis>
-            <limit effort="30" velocity="1.7453292519943"/>
+<?xml version="1.0" ?>
+<robot name="iisy">
+    <link name="world" />
+    <link name="base_link"/>
+    <link name="base">
+        <collision>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/base_bottom_coll.stl"/>
+            </geometry>
+        </collision>
+        <visual>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/base_bottom_low.stl"/>
+            </geometry>
+        </visual>
+
+        <inertial>
+            <origin xyz="0.025 0 0.05" rpy="0 0 0"/>
+            <mass value="6"/>
+            <inertia ixx="0.0032666666666667" ixy="0.0" ixz="0.0" iyy="0.0032666666666667" iyz="0.0" izz="0.0025"/>
+        </inertial>
+
+    </link>
+    <link name="base_top">
+        <collision>
+            <origin xyz="0 -0.005 0.08"/>
+            <geometry>
+                <box size="0.14 0.15 0.16"/>
+            </geometry>
+        </collision>
+        <visual>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/base_top_low.stl"/>
+            </geometry>
+        </visual>
+
+        <inertial>
+            <origin xyz="0 0 0.155" rpy="0 0 0"/>
+            <mass value="2"/>
+            <inertia ixx="0.0032666666666667" ixy="0.0" ixz="0.0" iyy="0.0032666666666667" iyz="0.0" izz="0.0025"/>
+        </inertial>
+
+    </link>
+    <link name="link1_full">
+        <collision>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/link_1_full_coll.stl"/>
+            </geometry>
+        </collision>
+        <visual>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/link_1_full_low.stl"/>
+            </geometry>
+        </visual>
+
+        <inertial>
+            <origin xyz="0 0 0.36" rpy="0 0 0"/>
+            <mass value="4"/>
+            <inertia ixx="0.0325" ixy="0.0" ixz="0.0" iyy="0.0325" iyz="0.0" izz="0.005"/>
+        </inertial>
+
+    </link>
+    <link name="link2_bottom">
+        <collision>
+            <origin xyz="0 0.07 0.025"/>
+            <geometry>
+                <box size="0.14 0.14 0.19"/>
+            </geometry>
+        </collision>
+        <visual>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/link_2_bottom_low.stl"/>
+            </geometry>
+        </visual>
+
+        <inertial>
+            <origin xyz="0 0 0.585" rpy="0 0 0"/>
+            <mass value="2"/>
+            <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.0025"/>
+        </inertial>
+
+    </link>
+    <link name="link2_top">
+        <collision>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/link_2_top_coll.stl"/>
+            </geometry>
+        </collision>
+        <visual>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/link_2_top_low.stl"/>
+            </geometry>
+        </visual>
+
+        <inertial>
+            <origin xyz="0 0 0.735" rpy="0 0 0"/>
+            <mass value="2"/>
+            <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.0025"/>
+        </inertial>
+
+    </link>
+    <link name="link3_bottom">
+        <collision>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/link_3_bottom_coll.stl"/>
+            </geometry>
+        </collision>
+        <visual>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/link_3_bottom_low.stl"/>
+            </geometry>
+        </visual>
+
+        <inertial>
+            <origin xyz="0 0 0.86" rpy="0 0 0"/>
+            <mass value="2"/>
+            <inertia ixx="0.0029166666666667" ixy="0.0" ixz="0.0" iyy="0.0029166666666667" iyz="0.0" izz="0.0025"/>
+        </inertial>
+
+    </link>
+    <link name="link3_top">
+        <collision>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/link_3_top_coll.stl"/>
+            </geometry>
+        </collision>
+        <visual>
+            <geometry>
+                <mesh filename="file://$(find gaz_simulation)/stl/link_3_top_low.stl"/>
+            </geometry>
+        </visual>
+
+        <inertial>
+            <origin xyz="0 0 0.94025" rpy="0 0 0"/>
+            <mass value="0.8"/>
+            <inertia ixx="0.00074401666666667" ixy="0.0" ixz="0.0" iyy="0.00074401666666667" iyz="0.0" izz="0.001"/>
+        </inertial>
+
+    </link>
+    <joint type="fixed" name="world_base_link_fixed">
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <parent link="world"/>
+        <child link="base_link"/>
+    </joint>
+    <joint type="fixed" name="base_base_link_fixed">
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <parent link="base_link"/>
+        <child link="base"/>
+    </joint>
+    <joint type="continuous" name="base_rot">
+        <origin xyz="0 0 0.1205" rpy="0 0 0"/>
+        <parent link="base"/>
+        <child link="base_top"/>
+        <axis xyz="0 0 1"/>
+        <limit effort="30" velocity="1.7453292519943"/>
+    </joint>
+    <joint type="revolute" name="base_link1_joint">
+        <origin xyz="0 -0.080499 0.092997" rpy="0 0 0"/>
+        <parent link="base_top"/>
+        <child link="link1_full"/>
+        <axis xyz="0 1 0"/>
+        <limit effort="30" velocity="1.7453292519943" lower="-2.2689280275926" upper="2.4434609527921"/>
+    </joint>
+    <joint type="revolute" name="link1_link2_joint">
+        <origin xyz="0 0 0.3" rpy="0 0 0"/>
+        <parent link="link1_full"/>
+        <child link="link2_bottom"/>
+        <axis xyz="0 1 0"/>
+        <limit effort="30" velocity="1.7453292519943" lower="-2.6179938779915" upper="2.6179938779915"/>
+    </joint>
+    <joint type="continuous" name="link2_rot">
+        <origin xyz="0 0.080501 0.120502" rpy="0 0 0"/>
+        <parent link="link2_bottom"/>
+        <child link="link2_top"/>
+        <axis xyz="0 0 1"/>
+        <limit effort="30" velocity="1.7453292519943"/>
+    </joint>
+    <joint type="continuous" name="link2_link3_joint">
+        <origin xyz="0 0.0565 0.178003" rpy="0 0 0"/>
+        <parent link="link2_top"/>
+        <child link="link3_bottom"/>
+        <axis xyz="0 1 0"/>
+        <limit effort="30" velocity="1.7453292519943"/>
+    </joint>
+    <joint type="continuous" name="link3_rot">
+        <origin xyz="0 -0.055001 0.085501" rpy="0 0 0"/>
+        <parent link="link3_bottom"/>
+        <child link="link3_top"/>
+        <axis xyz="0 0 1"/>
+        <limit effort="30" velocity="1.7453292519943"/>
+    </joint>
+    <gazebo>
+        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+            <robotNamespace>/simple_model</robotNamespace>
+        </plugin>
+    </gazebo>
+
+    <transmission name="trans_base_rot">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="base_rot">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        
-        <joint type="revolute" name="link1_joint">
-            <pose>0 0 -0.15 0 0 0</pose>
-            <parent link="base_rot"/>
-            <child link="link1"/>
-            <axis>
-                <xyz>1 0 0</xyz>
-            </axis>
-            <limit effort="30" velocity="1.7453292519943" lower="-1.221730476396" upper="1.221730476396"/>
+        <actuator name="base_rot_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_base_link1_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="base_link1_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        
-        <joint type="revolute" name="link2_rot_joint">
-            <pose>0 0 -0.075 0 0 0</pose>
-            <parent link="link1"/>
-            <child link="link2_rot"/>
-            <axis>
-                <xyz>1 0 0</xyz>
-            </axis>
-            <limit effort="30" velocity="1.7453292519943" lower="-1.3089969389957" upper="1.3089969389957"/>
+        <actuator name="base_link1_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_link1_link2_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="link1_link2_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        
-        <joint type="continuous" name="link2_joint">
-            <pose>0 0 -0.075 0 0 0</pose>
-            <parent link="link2_rot"/>
-            <child link="link2"/>
-            <axis>
-                <xyz>0 0 1</xyz>
-            </axis>
-            <limit effort="30" velocity="1.7453292519943"/>
+        <actuator name="link1_link2_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_link2_rot">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="link2_rot">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        
-        <joint type="revolute" name="link3_rot_joint">
-            <pose>0 0 -0.05 0 0 0</pose>
-            <parent link="link2"/>
-            <child link="link3_rot"/>
-            <axis>
-                <xyz>1 0 0</xyz>
-            </axis>
-            <limit effort="30" velocity="1.7453292519943" lower="-0.95993108859688" upper="0.95993108859688"/>
+        <actuator name="link2_rot_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_link2_link3_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="link2_link3_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        
-        <joint type="continuous" name="link3_joint">
-            <pose>0 0 -0.03025 0 0 0</pose>
-            <parent link="link3_rot"/>
-            <child link="link3"/>
-            <axis>
-                <xyz>0 0 1</xyz>
-            </axis>
-            <limit effort="30" velocity="1.7453292519943"/>
+        <actuator name="link2_link3_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_link3_rot">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="link3_rot">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
         </joint>
-        
-        <gazebo>
-            <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
-                <robotNamespace>/simple_model</robotNamespace>
-            </plugin>
-        </gazebo>
-</robot>
+        <actuator name="link3_rot_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <gazebo>
+        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+            <robotNamespace>/</robotNamespace>
+        </plugin>
+    </gazebo>
+</robot>
\ No newline at end of file
diff --git a/src/gaz_simulation/world/test2.xml b/src/gaz_simulation/world/test2.xml
index 9d47c94..7d0329c 100644
--- a/src/gaz_simulation/world/test2.xml
+++ b/src/gaz_simulation/world/test2.xml
@@ -111,7 +111,7 @@
             </box>
           </geometry>
         </visual>-->
-        <sensor name="laser" type="gpu_ray">
+        <sensor name="laser_1" type="gpu_ray">
           <pose>0.1 0 0 0 0 0</pose>
           <ray>
             <scan>
@@ -132,21 +132,21 @@
           <update_rate>10</update_rate>
           <visualize>true</visualize>
 
-          <plugin name='laser' filename='libgazebo_ros_ray_sensor.so'>
+	  <plugin name='laser_1_plugin' filename='libgazebo_ros_ray_sensor.so'>
             <ros>
               <namespace>/lidar_1</namespace>
               <argument>--ros-args --remap ~/out:=scan</argument>
             </ros>
             <output_type>sensor_msgs/PointCloud2</output_type>
-          </plugin>
+	  </plugin>
         </sensor>
       </link>
     </model>
     <model name="lidar_2">
       <static>1</static>
       <link name="lidar_2_link">
-        <pose>-2 1 1 0 0 </pose>
-        <collision name='collision'>
+        <pose>-1.9 1 1 0 0 </pose>
+	<!--<collision name='collision'>
           <geometry>
             <box>
               <size>.1 .1 .1</size>
@@ -159,9 +159,8 @@
               <size>.1 .1 .1</size>
             </box>
           </geometry>
-        </visual>
-        <sensor name="laser" type="gpu_ray">
-          <pose>0.1 0 0 0 0 0</pose>
+	</visual>-->
+        <sensor name="laser_2" type="gpu_ray">
           <ray>
             <scan>
               <horizontal>
@@ -181,7 +180,7 @@
           <update_rate>10</update_rate>
           <visualize>true</visualize>
 
-          <plugin name='laser' filename='libgazebo_ros_ray_sensor.so'>
+          <plugin name='laser_2_plugin' filename='libgazebo_ros_ray_sensor.so'>
             <ros>
               <namespace>/lidar_2</namespace>
               <frameName>/lidar_2_link</frameName>
diff --git a/src/gazebo_ros_actor/src/PosePublisher.cc b/src/gazebo_ros_actor/src/PosePublisher.cc
index 4d32235..3695adc 100644
--- a/src/gazebo_ros_actor/src/PosePublisher.cc
+++ b/src/gazebo_ros_actor/src/PosePublisher.cc
@@ -8,10 +8,10 @@
 
 class PosePublisher : public rclcpp::Node{
 public:
-    PosePublisher(): Node("minimal_publisher") {
-        publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("currentPose", 10);
+    PosePublisher(): Node("gazebo_actor_current") {
+        publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("currentPose",10);
     }
-    void update(ignition::math::Pose3d newPose){
+    void update(const ignition::math::Pose3d& newPose){
         geometry_msgs::msg::Pose pose;
         pose.position.x=newPose.X();
         pose.position.y=newPose.Y();
diff --git a/src/gazebo_ros_actor/src/RosActorPlugin.cc b/src/gazebo_ros_actor/src/RosActorPlugin.cc
index ebeb602..cff83fb 100644
--- a/src/gazebo_ros_actor/src/RosActorPlugin.cc
+++ b/src/gazebo_ros_actor/src/RosActorPlugin.cc
@@ -38,16 +38,16 @@ RosActorPlugin::RosActorPlugin()
 
 class PoseSubscriber : public rclcpp::Node{
     public:
-        explicit PoseSubscriber(RosActorPlugin* plugin): Node("targetPose"){
+        explicit PoseSubscriber(RosActorPlugin* plugin): Node("gazebo_actor_target"){
             this->plugin = plugin;
         }
 
     private:
         RosActorPlugin* plugin;
         rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr subscription_ = this->create_subscription<geometry_msgs::msg::Pose>(
-            "actorTarget", 10, [this](const geometry_msgs::msg::Pose PH1) {
+            "targetPose", 10, [this](const geometry_msgs::msg::Pose PH1) {
                 RCLCPP_INFO(this->get_logger(), "I heard: '%f %f %f'", PH1.position.x,PH1.position.y,PH1.position.z);
-                plugin->target.Set(PH1.position.x,PH1.position.y,PH1.position.z);
+                plugin->target.Set(PH1.position.x,PH1.position.y,1.0);
             });
 };
 
@@ -160,6 +160,9 @@ void RosActorPlugin::HandleObstacles(ignition::math::Vector3d &_pos)
 /////////////////////////////////////////////////
 void RosActorPlugin::OnUpdate(const common::UpdateInfo &_info)
 {
+    if(target.X()==HUGE_VAL || target.Y() == HUGE_VAL){
+        return;
+    }
     // Time delta
     double dt = (_info.simTime - this->lastUpdate).Double();
 
@@ -169,7 +172,7 @@ void RosActorPlugin::OnUpdate(const common::UpdateInfo &_info)
 
     double distance = pos.Length();
 
-    if (distance < 0.3)
+    if (distance < 0.01)
     {
         return;
     }
@@ -178,7 +181,7 @@ void RosActorPlugin::OnUpdate(const common::UpdateInfo &_info)
     pos = pos.Normalize() * this->targetWeight;
 
     // Adjust the direction vector by avoiding obstacles
-    this->HandleObstacles(pos);
+    // this->HandleObstacles(pos);
 
     // Compute the yaw orientation
     ignition::math::Angle yaw = atan2(pos.Y(), pos.X()) + 1.5707 - rpy.Z();
diff --git a/src/moveit2_tutorials b/src/moveit2_tutorials
new file mode 160000
index 0000000..e02236a
--- /dev/null
+++ b/src/moveit2_tutorials
@@ -0,0 +1 @@
+Subproject commit e02236a8096f540c356504a5ceeb1a08212421e9
diff --git a/src/moveit_visual_tools b/src/moveit_visual_tools
new file mode 160000
index 0000000..ef16670
--- /dev/null
+++ b/src/moveit_visual_tools
@@ -0,0 +1 @@
+Subproject commit ef16670fab41acb5a9f265ce2de6d7875c6358eb