From 72a37287eb5c07e9bbec29de6eafe6c4e71bd339 Mon Sep 17 00:00:00 2001
From: Bastian Hofmann <bastian.hofmann@xitaso.com>
Date: Tue, 27 Jun 2023 12:18:48 +0000
Subject: [PATCH] Hopefully last fixes to colors of robot and trees

---
 src/btree/CMakeLists.txt                   |  3 +-
 src/btree/src/Extensions.hpp               |  2 +-
 src/btree/src/Tree.cpp                     |  2 +-
 src/btree/trees/actorTreeColab.xml         |  6 +--
 src/btree/trees/actorTreeCoop.xml          |  1 +
 src/btree/trees/robotTreeCoex.xml          | 25 -----------
 src/iisy_config/config/iisy.urdf           | 51 +++++++++++++++++++---
 src/ros_actor_action_server/src/Server.cpp |  4 +-
 8 files changed, 54 insertions(+), 40 deletions(-)

diff --git a/src/btree/CMakeLists.txt b/src/btree/CMakeLists.txt
index 7db86a1..c427d62 100644
--- a/src/btree/CMakeLists.txt
+++ b/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
diff --git a/src/btree/src/Extensions.hpp b/src/btree/src/Extensions.hpp
index f1835aa..3dbb908 100644
--- a/src/btree/src/Extensions.hpp
+++ b/src/btree/src/Extensions.hpp
@@ -64,7 +64,7 @@ namespace BT {
     }
 
     template<> inline
-    std::shared_ptr<geometry_msgs::msg::Point> BT::convertFromString(StringView str) {
+    std::shared_ptr<geometry_msgs::msg::Point> convertFromString(StringView str) {
         auto parts = splitString(str, ',');
         if (parts.size() != 3) {
             throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
diff --git a/src/btree/src/Tree.cpp b/src/btree/src/Tree.cpp
index 285ed5d..0b04fbd 100644
--- a/src/btree/src/Tree.cpp
+++ b/src/btree/src/Tree.cpp
@@ -78,7 +78,7 @@ int main(int argc, char **argv) {
     NodeBuilder builderActorMovement = [&actorMovementNode, &blackboard, &blackboardMutex](const std::string &name, const NodeConfiguration &config) {
         return std::make_unique<ActorMovement>(name, config, actorMovementNode, "/ActorPlugin/movement",[&blackboard,&blackboardMutex](std::shared_ptr<const Movement::Feedback> feedback){
             blackboardMutex.lock();
-            //blackboard->set("actorPos", std::make_shared<geometry_msgs::msg::Pose>(feedback->current));
+            blackboard->set("actorPos", std::make_shared<geometry_msgs::msg::Pose>(feedback->current));
             blackboardMutex.unlock();
         });
     };
diff --git a/src/btree/trees/actorTreeColab.xml b/src/btree/trees/actorTreeColab.xml
index acabedc..70bbd31 100644
--- a/src/btree/trees/actorTreeColab.xml
+++ b/src/btree/trees/actorTreeColab.xml
@@ -8,11 +8,11 @@
                 </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>
+                    </WeightedRandom>
                     <Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="{actorTarget}"/>
                 </Sequence>
             </ReactiveSequence>
@@ -21,7 +21,7 @@
                 <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_put_back"/>
+                <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"/>
diff --git a/src/btree/trees/actorTreeCoop.xml b/src/btree/trees/actorTreeCoop.xml
index 4e2390a..594dd09 100644
--- a/src/btree/trees/actorTreeCoop.xml
+++ b/src/btree/trees/actorTreeCoop.xml
@@ -17,6 +17,7 @@
             <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">
diff --git a/src/btree/trees/robotTreeCoex.xml b/src/btree/trees/robotTreeCoex.xml
index 34b7182..c403b0a 100644
--- a/src/btree/trees/robotTreeCoex.xml
+++ b/src/btree/trees/robotTreeCoex.xml
@@ -20,29 +20,4 @@
             </Control>
         </ReactiveSequence>
     </BehaviorTree>
-    <BehaviorTree ID="Slowdown">
-        <ReactiveSequence>
-            <Inverter>
-                <Condition ID="InAreaTest" area="{unsafeArea}" pose="{actorPos}"/>
-            </Inverter>
-            <IfThenElse>
-                <Condition ID="InAreaTest" area="{warningArea}" pose="{actorPos}"/>
-                <Action ID="SetRobotVelocity" velocity="0.1"/>
-                <Action ID="SetRobotVelocity" velocity="1"/>
-            </IfThenElse>
-            <SubTree ID="SafeTree" _autoremap="1"/>
-        </ReactiveSequence>
-    </BehaviorTree>
-    <BehaviorTree ID="SafeTree">
-        <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>
-    </BehaviorTree>
 </root>
\ No newline at end of file
diff --git a/src/iisy_config/config/iisy.urdf b/src/iisy_config/config/iisy.urdf
index 0021227..cff60f5 100644
--- a/src/iisy_config/config/iisy.urdf
+++ b/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>
diff --git a/src/ros_actor_action_server/src/Server.cpp b/src/ros_actor_action_server/src/Server.cpp
index ac981f2..facc5ae 100644
--- a/src/ros_actor_action_server/src/Server.cpp
+++ b/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;
 }