yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Hopefully last fixes to colors of robot and trees

Browse Source
This commit is contained in:
Bastian Hofmann 2023-06-27 12:18:48 +00:00
parent 88100990af
commit 72a37287eb
8 changed files with 54 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/btree/CMakeLists.txt
View File

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

2
src/btree/src/Extensions.hpp
View File

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

2
src/btree/src/Tree.cpp
View File

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

6
src/btree/trees/actorTreeColab.xml
View File

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

1
src/btree/trees/actorTreeCoop.xml
View File

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

25
src/btree/trees/robotTreeCoex.xml
View File

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

51
src/iisy_config/config/iisy.urdf
View File

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

4
src/ros_actor_action_server/src/Server.cpp
View File

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