Hopefully last fixes to colors of robot and trees

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

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>'.");

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();
         });
     };

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"/>

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">

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>

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/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;
 }