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

Fixed build

Browse Source
This commit is contained in:
Bastian Hofmann 2023-06-18 01:33:07 +00:00
parent a26d82f4fe
commit 88100990af
10 changed files with 103 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
build.sh
View File

@ -1,4 +1,4 @@
#!/bin/bash
pushd "$(dirname "$0")" || exit
colcon build --event-handlers console_cohesion+ --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -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

4
src/btree/CMakeLists.txt
View File

@ -36,7 +36,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 +48,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})

93
src/btree/src/Extensions.cpp
View File

@ -1,93 +0,0 @@
//
// Created by bastian on 28.02.22.
//
#include "Extensions.hpp"
namespace BT {
template<> [[nodiscard]]
std::shared_ptr<Position2D> convertFromString(StringView str) {
auto split = splitString(str, ',');
if (split.size() != 2) {
throw RuntimeError("Incorrect number of arguments, expected 2 in format '<x>,<y>'.");
}
auto pos = std::make_shared<Position2D>();
pos->x = convertFromString<double>(split[0]);
pos->y = convertFromString<double>(split[1]);
return pos;
}
template<> [[nodiscard]]
std::shared_ptr<Area> convertFromString(StringView str) {
auto parts = splitString(str, '|');
if (parts.size() < 3) {
throw RuntimeError("Incorrect number of arguments, expected at least 3 in format '<x>,<y>,<z>|<x>,<y>,<z>|<x>,<y>,<z>'.");
}
auto output = std::make_shared<Area>();
std::vector<Position2D> array(parts.size());
for (unsigned long i = 0; i < parts.size(); ++i) {
array[i] = *convertFromString<std::shared_ptr<Position2D>>(parts[i]);
}
output->triangles = array;
return output;
}
template<> [[nodiscard]]
std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
auto parts = splitString(str, ',');
if (!(parts.size() == 3 || parts.size() == 7)) {
throw RuntimeError("Incorrect number of arguments, expected 3 or 7 in format '<x>,<y>,<z>[,<qw>,<qx>,<qy>,<qz>]'.");
}
auto pose = std::make_shared<geometry_msgs::msg::Pose>();
if(parts.size() == 7){
pose->orientation.w = convertFromString<double>(parts[3]);
pose->orientation.x = convertFromString<double>(parts[4]);
pose->orientation.y = convertFromString<double>(parts[5]);
pose->orientation.z = convertFromString<double>(parts[6]);
}else{
pose->orientation.w = 1;
pose->orientation.x = 0;
pose->orientation.y = 0;
pose->orientation.z = 0;
}
pose->position.x = convertFromString<double>(parts[0]);
pose->position.y = convertFromString<double>(parts[1]);
pose->position.z = convertFromString<double>(parts[2]);
return pose;
}
template<> [[nodiscard]]
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>'.");
}
auto point = std::make_shared<geometry_msgs::msg::Point>();
point->x = convertFromString<double>(parts[0]);
point->y = convertFromString<double>(parts[1]);
point->z = convertFromString<double>(parts[2]);
return point;
}
template<> [[nodiscard]]
std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str) {
auto parts = splitString(str, ',');
if (parts.size() != 4) {
throw RuntimeError("Incorrect number of arguments, expected 4 in format '<w>,<x>,<y>,<z>'.");
}
auto point = std::make_shared<geometry_msgs::msg::Quaternion>();
point->w = convertFromString<double>(parts[0]);
point->x = convertFromString<double>(parts[1]);
point->y = convertFromString<double>(parts[2]);
point->z = convertFromString<double>(parts[3]);
return point;
}
}

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

@ -8,20 +8,91 @@
#include <geometry_msgs/msg/pose.hpp>
namespace BT {
template<> [[nodiscard]]
std::shared_ptr<Position2D> convertFromString(StringView str);
template<> [[nodiscard]]
std::shared_ptr<Area> convertFromString(StringView str);
template<> [[nodiscard]]
std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str);
template<> [[nodiscard]]
std::shared_ptr<geometry_msgs::msg::Point> convertFromString(StringView str);
template<> [[nodiscard]]
std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str);
template<> inline
std::shared_ptr<Position2D> convertFromString(StringView str) {
auto split = splitString(str, ',');
if (split.size() != 2) {
throw RuntimeError("Incorrect number of arguments, expected 2 in format '<x>,<y>'.");
}
auto pos = std::make_shared<Position2D>();
pos->x = convertFromString<double>(split[0]);
pos->y = convertFromString<double>(split[1]);
return pos;
}
template<> inline
std::shared_ptr<Area> convertFromString(StringView str) {
auto parts = splitString(str, '|');
if (parts.size() < 3) {
throw RuntimeError("Incorrect number of arguments, expected at least 3 in format '<x>,<y>,<z>|<x>,<y>,<z>|<x>,<y>,<z>'.");
}
auto output = std::make_shared<Area>();
std::vector<Position2D> array(parts.size());
for (unsigned long i = 0; i < parts.size(); ++i) {
array[i] = *convertFromString<std::shared_ptr<Position2D>>(parts[i]);
}
output->triangles = array;
return output;
}
template<> inline
std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
auto parts = splitString(str, ',');
if (!(parts.size() == 3 || parts.size() == 7)) {
throw RuntimeError("Incorrect number of arguments, expected 3 or 7 in format '<x>,<y>,<z>[,<qw>,<qx>,<qy>,<qz>]'.");
}
auto pose = std::make_shared<geometry_msgs::msg::Pose>();
if(parts.size() == 7){
pose->orientation.w = convertFromString<double>(parts[3]);
pose->orientation.x = convertFromString<double>(parts[4]);
pose->orientation.y = convertFromString<double>(parts[5]);
pose->orientation.z = convertFromString<double>(parts[6]);
}else{
pose->orientation.w = 1;
pose->orientation.x = 0;
pose->orientation.y = 0;
pose->orientation.z = 0;
}
pose->position.x = convertFromString<double>(parts[0]);
pose->position.y = convertFromString<double>(parts[1]);
pose->position.z = convertFromString<double>(parts[2]);
return pose;
}
template<> inline
std::shared_ptr<geometry_msgs::msg::Point> BT::convertFromString(StringView str) {
auto parts = splitString(str, ',');
if (parts.size() != 3) {
throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
}
auto point = std::make_shared<geometry_msgs::msg::Point>();
point->x = convertFromString<double>(parts[0]);
point->y = convertFromString<double>(parts[1]);
point->z = convertFromString<double>(parts[2]);
return point;
}
template<> inline
std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str) {
auto parts = splitString(str, ',');
if (parts.size() != 4) {
throw RuntimeError("Incorrect number of arguments, expected 4 in format '<w>,<x>,<y>,<z>'.");
}
auto point = std::make_shared<geometry_msgs::msg::Quaternion>();
point->w = convertFromString<double>(parts[0]);
point->x = convertFromString<double>(parts[1]);
point->y = convertFromString<double>(parts[2]);
point->z = convertFromString<double>(parts[3]);
return point;
}
}
#endif //BUILD_EXTENSIONS_H

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

2
src/btree/src/nodes/GenerateXYPose.cpp
View File

@ -82,6 +82,8 @@ BT::NodeStatus BT::GenerateXYPose::tick() {
randomPose->orientation.x = 0;
randomPose->orientation.y = 0;
randomPose->orientation.z = 0;
std::cout << "Generated Pose." << std::endl;
setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", randomPose);
return NodeStatus::SUCCESS;

1
src/btree/src/nodes/InAreaTest.cpp
View File

@ -3,6 +3,7 @@
//
#include "InAreaTest.h"
#include "../Extensions.hpp"
BT::PortsList BT::InAreaTest::providedPorts() {
return {

5
src/btree/src/nodes/InterruptableSequence.cpp
View File

@ -15,11 +15,10 @@ void BT::InterruptableSequence::halt() {
BT::NodeStatus BT::InterruptableSequence::tick() {
auto result = children_nodes_[position]->executeTick();
setStatus(NodeStatus::RUNNING);
if(result==NodeStatus::FAILURE){
resetChildren();
position=0;
setStatus(NodeStatus::FAILURE);
return NodeStatus::FAILURE;
}
@ -28,10 +27,12 @@ BT::NodeStatus BT::InterruptableSequence::tick() {
if(position>=children_nodes_.size()){
resetChildren();
position=0;
setStatus(NodeStatus::SUCCESS);
return NodeStatus::SUCCESS;
}
}
setStatus(NodeStatus::RUNNING);
return NodeStatus::RUNNING;
}

2
src/btree/src/nodes/OffsetPose.cpp
View File

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

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

@ -11,14 +11,12 @@
<Action ID="SetRobotVelocity" velocity="1"/>
</IfThenElse>
<Control ID="InterruptableSequence">
<Fallback>
<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}"/>
</Fallback>
<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>