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

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

@@ -2,11 +2,10 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 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,7 +35,6 @@ endforeach()
 
 set(CPP_FILES
         src/Tree.cpp
-        src/Extensions.cpp
         src/nodes/WeightedRandomNode.cpp
         src/nodes/GenerateXYPose.cpp
         src/nodes/RobotMove.cpp
@@ -49,8 +47,9 @@ set(CPP_FILES
         src/nodes/ActorAnimation.cpp
         src/nodes/ActorMovement.cpp
         )
-
 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 ${DEPENDENCIES})
 

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,7 +37,7 @@ 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 || parts.size() == 7)) {
@@ -61,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) {
@@ -76,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) {
@@ -91,4 +93,6 @@ namespace BT {
 
         return point;
     }
-}
+}
+
+#endif //BUILD_EXTENSIONS_H

src/btree/src/Tree.cpp

@@ -1,7 +1,10 @@
+#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>
@@ -20,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);
     
@@ -51,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

@@ -18,7 +18,7 @@ BT::PortsList ActorAnimation::providedPorts() {
 }
 
 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;
 
@@ -37,8 +37,10 @@ BT::NodeStatus ActorAnimation::onStart() {
     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();
@@ -53,14 +55,12 @@ BT::NodeStatus ActorAnimation::onStart() {
 BT::NodeStatus ActorAnimation::onRunning() {
     mutex.lock();
     auto status = result;
-    if(result != BT::NodeStatus::RUNNING){
-        result = BT::NodeStatus::IDLE;
-    }
     mutex.unlock();
     return status;
 }
 
 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>
@@ -22,7 +23,7 @@ BT::PortsList ActorMovement::providedPorts() {
 }
 
 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;
 
@@ -51,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();
@@ -81,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

@@ -82,8 +82,9 @@ BT::NodeStatus BT::GenerateXYPose::tick() {
     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,6 +2,7 @@
 // Created by bastian on 29.03.22.
 //
 
+#include "../Extensions.hpp"
 #include "OffsetPose.h"
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
 #include <memory>
@@ -23,6 +24,7 @@ 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;
     }
     

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;
 }
@@ -20,7 +20,7 @@ BT::NodeStatus BT::RobotMove::onStart() {
     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;
@@ -42,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

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <root main_tree_to_execute="actorTree">
     <BehaviorTree ID="actorTree">
-        <Control ID="WeightedRandom" weights="95,5">
+        <WeightedRandom weights="95,5">
             <Sequence>
                 <SubTree ID="WorkOnBench"/>
                 <SubTree ID="DepositToShelf"/>
@@ -10,7 +10,7 @@
                 <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
                 <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="{actorTarget}"/>
             </Sequence>
-        </Control>
+        </WeightedRandom>
     </BehaviorTree>
     <BehaviorTree ID="DepositToShelf">
         <Sequence>

src/btree/trees/actorTreeColab.xml

@@ -8,17 +8,24 @@
                 </Inverter>
 
                 <Sequence>
-                    <Control ID="WeightedRandom" weights="100,5,2">
+                    <WeightedRandom weights="100,5,2">
                         <Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
                         <Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
                         <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
-                    </Control>
-                    <Action ID="ActorMovement" animation_name="standing_walk" target="{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_name="standing_walk" target="{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>

src/btree/trees/actorTreeCoop.xml

@@ -1,26 +1,52 @@
 <?xml version="1.0"?>
 <root main_tree_to_execute="actorTree">
     <BehaviorTree ID="actorTree">
-        <Fallback>
-            <ReactiveSequence>
-                <Inverter>
-                    <Condition ID="IsCalled"/>
-                </Inverter>
+        <Sequence>
+            <Control ID="WeightedRandom" weights="95,5">
                 <Sequence>
-                    <Control ID="WeightedRandom" weights="100,5,2">
-                        <Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
-                        <Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
-                        <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
-                    </Control>
-                    <Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
+                    <SubTree ID="WorkOnBench"/>
+                    <SubTree ID="DepositToShelf"/>
                 </Sequence>
-            </ReactiveSequence>
-            <Sequence>
-                <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
-                <Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
-                <Action ID="SetCalledTo" state="false"/>
-            </Sequence>
-        </Fallback>
+                <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

@@ -10,16 +10,14 @@
                 <Action ID="SetRobotVelocity" velocity="0.1"/>
                 <Action ID="SetRobotVelocity" velocity="1"/>
             </IfThenElse>
-            <Fallback>
-                <Control ID="InterruptableSequence">
-                    <Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/>
-                    <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
-                    <Action ID="RobotMove" target="{target}"/>
-                    <Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/>
-                    <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
-                    <Action ID="RobotMove" target="{target}"/>
-                </Control>
-            </Fallback>
+            <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

@@ -11,16 +11,16 @@
                 <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" 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"/>
-                </Control>
+                <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>

src/controller_test/src/test.cpp

@@ -12,192 +12,178 @@
 
 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_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_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;
+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);
+int main(int argc, char * argv[]){
+    rclcpp::init(argc, argv);
 
-  node = std::make_shared<rclcpp::Node>("trajectory_test_node");
+    node = std::make_shared<rclcpp::Node>("trajectory_test_node");
 
-  std::cout << "node created" << std::endl;
+    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");
+    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;
+    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<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());
+    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;
+    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());
+    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;
+    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());
+    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;
+    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());
+    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;
+    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);
+    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());
+    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;
     }
-  };
-  //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)); };
+    RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
 
-  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;
+    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");
 
-  auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);
+    // 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;
+    }
 
-  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;
+    std::cout << "async_send_goal" << std::endl;
   
-  rclcpp::shutdown();
+    rclcpp::shutdown();
 
-  return 0;
+    return 0;
 }

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) {
@@ -184,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;

src/ign_world/launch/gazebo_controller_launch.py

@@ -1,7 +1,7 @@
 import os
 from ament_index_python import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, 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
@@ -128,8 +128,13 @@ def generate_launch_description():
             output='both',
             emulate_tty=True,
         ),
-
-        btree,
+        
+        TimerAction(
+            period=10.0,
+            actions=[
+                btree,
+            ]
+        ),
 
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(

src/ign_world/world/world.sdf

@@ -9,18 +9,28 @@
         <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">
+        <light type="point" name="point_light_1">
             <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>
+            <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">
@@ -30,7 +40,7 @@
                 <rot>180</rot>
             </plugin>
             <skin>
-                <filename>$(find ign_actor_plugin)/animation/walk.dae</filename>
+                <filename>$(find ign_actor_plugin)/animation/standing_walk.dae</filename>
                 <scale>1.0</scale>
             </skin>
             

src/iisy_config/config/iisy.urdf

@@ -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,14 +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;
             }
@@ -137,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;
@@ -161,7 +167,7 @@ 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.animationSpeed = movementGoal->animation_distance;
@@ -169,7 +175,7 @@ int main(int argc, char **argv) {
 
                 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;
             }
@@ -179,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;

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}}}'