Removed cruft, added area parameter

src/btree_nodes/CMakeLists.txt

@@ -42,7 +42,6 @@ set(CPP_FILES
         src/nodes/AmICalled.cpp
         src/nodes/GenerateXYPose.cpp
         src/nodes/MoveActorToTarget.cpp
-        src/helpers/MinimalSubscriber.cpp
         src/nodes/RobotMove.cpp
         src/nodes/MoveConnection.cpp
         src/nodes/OffsetPose.cpp

src/btree_nodes/src/Extensions.cpp

@@ -7,67 +7,58 @@
 
 namespace BT {
     template<>
-    Position2D convertFromString(StringView str) {
+    std::shared_ptr<Position2D> convertFromString(StringView str) {
-        printf("Converting string: \"%s\"\n", str.data());
+        auto split = splitString(str, ',');
-
-        auto split = splitString(str, ';');
         if (split.size() != 2) {
-            throw RuntimeError("2 Arguments required.");
+            throw RuntimeError("Incorrect number of arguments, expected 2 in format '<x>,<y>'.");
         }
 
-        Position2D pos{};
+        auto pos = std::make_shared<Position2D>();
-        pos.x = convertFromString<double>(split[0]);
+        pos->x = convertFromString<double>(split[0]);
-        pos.y = convertFromString<double>(split[1]);
+        pos->y = convertFromString<double>(split[1]);
         return pos;
     }
 
     template<>
-    Area convertFromString(StringView str) {
+    std::shared_ptr<Area> convertFromString(StringView str) {
-        printf("Converting string: \"%s\"\n", str.data());
-
         auto parts = splitString(str, '|');
         if (parts.size() < 3) {
-            throw RuntimeError("Not enough argument pairs.");
+            throw RuntimeError("Incorrect number of arguments, expected at least 3 in format '<x>,<y>,<z>|<x>,<y>,<z>|<x>,<y>,<z>'.");
         }
 
-        Area output{};
+        auto output = std::make_shared<Area>();
         std::vector<Position2D> array(parts.size());
         for (unsigned long i = 0; i < parts.size(); ++i) {
-            array[i] = convertFromString<Position2D>(parts[i]);
+            array[i] = *convertFromString<std::shared_ptr<Position2D>>(parts[i]);
         }
-        output.triangles = array;
+        output->triangles = array;
         return output;
     }
 
     template<>
     std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
-
+        auto parts = splitString(str, ',');
-        std::cout << "[ generating pose from string ]" << std::endl;
-
-        auto parts = splitString(str, '|');
         if (parts.size() != 3) {
-            throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>|<y>|<z>'.");
+            throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
         }
 
         auto pose = std::make_shared<geometry_msgs::msg::Pose>();
-        pose->orientation.w = 0;
+        pose->orientation.w = 1;
         pose->orientation.x = 0;
-        pose->orientation.y = 1;
+        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]);
 
-        std::cout << "[ generated pose from string ]" << std::endl;
-
         return pose;
     }
 
     template<>
     std::shared_ptr<geometry_msgs::msg::Point> convertFromString(StringView str) {
-        auto parts = splitString(str, '|');
+        auto parts = splitString(str, ',');
         if (parts.size() != 3) {
-            throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>|<y>|<z>'.");
+            throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
         }
 
         auto point = std::make_shared<geometry_msgs::msg::Point>();
@@ -80,9 +71,9 @@ namespace BT {
 
     template<>
     std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str) {
-        auto parts = splitString(str, '|');
+        auto parts = splitString(str, ',');
         if (parts.size() != 4) {
-            throw RuntimeError("Incorrect number of arguments, expected 4 in format '<w>|<x>|<y>|<z>'.");
+            throw RuntimeError("Incorrect number of arguments, expected 4 in format '<w>,<x>,<y>,<z>'.");
         }
 
         auto point = std::make_shared<geometry_msgs::msg::Quaternion>();

src/btree_nodes/src/Tree.cpp

@@ -1,6 +1,8 @@
 #include "Area.h"
+#include <behaviortree_cpp_v3/basic_types.h>
 #include <behaviortree_cpp_v3/blackboard.h>
 #include <behaviortree_cpp_v3/bt_factory.h>
+#include <rclcpp/logging.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <random>
 #include <rclcpp/utilities.hpp>
@@ -10,7 +12,6 @@
 #include "nodes/GenerateXYPose.h"
 #include "nodes/MoveActorToTarget.h"
 #include "nodes/RobotMove.h"
-#include "helpers/MinimalSubscriber.h"
 #include "nodes/OffsetPose.h"
 #include "nodes/RandomFailure.h"
 #include "nodes/InAreaTest.h"
@@ -24,9 +25,12 @@
 int main(int argc, char **argv) {
     rclcpp::init(argc, argv);
     rclcpp::NodeOptions node_options;
-    node_options.automatically_declare_parameters_from_overrides(true);
 
     auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options);
+    
+    mainNode -> declare_parameter("trees",std::vector<std::string>({}));
+    mainNode -> declare_parameter("areas",std::vector<std::string>({}));
+    
     auto blackboard = Blackboard::create();
     auto blackboardMutex = std::mutex();
 
@@ -34,50 +38,6 @@ int main(int argc, char **argv) {
 
     BehaviorTreeFactory factory;
 
-    const std::shared_ptr<Area> safeArea = std::make_shared<Area>();
-    safeArea->triangles = std::vector<Position2D>({
-            {   1,     3.5 },
-            {   1,     7   },
-            {   3,     3.5 },
-            {   7,     7   },
-            {   3,    -1   },
-            {   7,    -1   }
-    });
-
-    const std::shared_ptr<Area> warningArea = std::make_shared<Area>();
-    warningArea->triangles = std::vector<Position2D>({
-            {  -1,     2.5 },
-            {  -1,     3.5 },
-            {   2,     2.5 },
-            {   3,     3.5 },
-            {   2,    -1   },
-            {   3,    -1   }
-    });
-
-    const std::shared_ptr<Area> unsafeArea = std::make_shared<Area>();
-    unsafeArea->triangles = std::vector<Position2D>({
-            {  -1,     1.5 },
-            {  -1,     2.5 },
-            {   1,     1.5 },
-            {   2,     2.5 },
-            {   1,    -1   },
-            {   2,    -1   }
-    });
-
-    const std::shared_ptr<Area> negativeYTable = std::make_shared<Area>();
-    negativeYTable->triangles = std::vector<Position2D>({
-            {   0.3,   -0.25    },
-            {  -0.3,   -0.25    },
-            {   0,     -0.4     }
-    });
-
-    const std::shared_ptr<Area> positiveYTable = std::make_shared<Area>();
-    positiveYTable->triangles = std::vector<Position2D>({
-            {   0.3,    0.25    },
-            {  -0.3,    0.25    },
-            {     0,    0.4     }
-    });
-
     std::cout << "Registering nodes." << std::endl;
 
     factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
@@ -127,24 +87,29 @@ int main(int argc, char **argv) {
         return called ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
     };
 
-    auto SetCalledTo = [&called](TreeNode& parent_node) -> NodeStatus{
+    auto SetCalledTo = [&called,&mainNode](TreeNode& parent_node) -> NodeStatus{
         if(!parent_node.getInput<bool>("state",called)){
-            std::cout<<"no state supplied."<<std::endl;
+            RCLCPP_DEBUG_STREAM(mainNode->get_logger(),"no state supplied.");
             return NodeStatus::FAILURE;
         }
-        std::cout<<"state changed"<<std::endl;
+        RCLCPP_DEBUG_STREAM(mainNode->get_logger(),"state changed.");
         return NodeStatus::SUCCESS;
     };
 
     factory.registerSimpleCondition("IsCalled",IsCalled);
     factory.registerSimpleAction("SetCalledTo",SetCalledTo,{InputPort("state","state to set called to")});
     
-    std::cout << "Creating tree." << std::endl;
+    RCLCPP_DEBUG_STREAM(mainNode->get_logger(), "Creating tree.");
     
-    mainNode -> declare_parameter("trees",std::vector<std::string>({
+    auto areaDefinitions = mainNode -> get_parameter("areas").as_string_array();
-        "/home/ros/workspace/src/btree_nodes/actorTree.xml",
+    if(areaDefinitions.size()%2==1){
-        "/home/ros/workspace/src/btree_nodes/robotTree.xml"
+        RCLCPP_ERROR_STREAM(mainNode->get_logger(), "Expected area syntax is \"name\",\"values\"");
-    }));
+        return -1;
+    }
+    for (unsigned long index = 0; index<areaDefinitions.size(); index+=2) {
+        RCLCPP_INFO_STREAM(mainNode->get_logger(), "Got area \""<< areaDefinitions[index] <<"\"");
+        blackboard->set(areaDefinitions[index], BT::convertFromString<std::shared_ptr<Area>>(areaDefinitions[index+1]));
+    }
     
     auto treeFileNames = mainNode -> get_parameter("trees").as_string_array();
     auto trees = std::vector<std::shared_ptr<BT::Tree>>();
@@ -156,23 +121,18 @@ int main(int argc, char **argv) {
     init->position.x = 6.0;
     init->position.y = 6.0;
 
-    blackboard->set("safeArea",safeArea);
-    blackboard->set("warningArea",warningArea);
-    blackboard->set("unsafeArea",unsafeArea);
-    blackboard->set("negativeYTable",negativeYTable);
-    blackboard->set("positiveYTable",positiveYTable);
     blackboard->set("actorPos", init);
 
-    std::cout << "Starting subscriber." << std::endl;
+    RCLCPP_INFO(mainNode->get_logger(), "Starting subscriber.");
 
-    std::thread([&executor]() {
+    std::thread([&executor,&mainNode]() {
         while(rclcpp::ok()){
             executor.spin();
         }
-        std::cout << "Executor stopped." << std::endl;
+        RCLCPP_WARN(mainNode->get_logger(), "Executor stopped.");
     }).detach();
     
-    std::cout << "Looping." << std::endl;
+    RCLCPP_INFO(mainNode->get_logger(), "Looping.");
     
     while (rclcpp::ok()) {
         blackboardMutex.lock();
@@ -182,7 +142,7 @@ int main(int argc, char **argv) {
         blackboardMutex.unlock();
     }
 
-    std::cout << "End." << std::endl;
+    RCLCPP_WARN(mainNode->get_logger(), "End.");
     
     rclcpp::shutdown();
 

src/btree_nodes/src/helpers/MinimalSubscriber.cpp

@@ -1,6 +0,0 @@
-//
-// Created by bastian on 26.03.22.
-//
-
-#include "MinimalSubscriber.h"
-

src/btree_nodes/src/helpers/MinimalSubscriber.h

@@ -1,45 +0,0 @@
-//
-// Created by bastian on 26.03.22.
-//
-
-#ifndef BUILD_MINIMALSUBSCRIBER_H
-#define BUILD_MINIMALSUBSCRIBER_H
-
-#include <memory>
-#include <geometry_msgs/msg/pose.hpp>
-#include "rclcpp/rclcpp.hpp"
-
-template<typename T>
-class MinimalSubscriber : public rclcpp::Node {
-public:
-    MinimalSubscriber(const std::string &node_name, const std::string &topic_name,
-                      std::function<void(T)> callback);
-
-public:
-    static std::thread createAsThread(const std::string &node_name, const std::string &topic_name,
-                                      std::function<void(T)> callback);
-
-private:
-    typename rclcpp::Subscription<T>::SharedPtr subscription_;
-};
-
-template<typename T>
-MinimalSubscriber<T>::MinimalSubscriber(const std::string &node_name, const std::string &topic_name,
-                                        std::function<void(T)> callback) :
-        Node(node_name) {
-    this->subscription_ = this->create_subscription<T>(
-            topic_name, 10, [callback](const T result) {
-                callback(result);
-            });
-}
-
-template<typename T>
-std::thread MinimalSubscriber<T>::createAsThread(const std::string &node_name, const std::string &topic_name,
-                                                 std::function<void(T)> callback) {
-    auto subscriber = std::make_shared<MinimalSubscriber<T>>(node_name, topic_name, callback);
-    return std::thread([subscriber]() {
-        rclcpp::spin(subscriber);
-    });
-}
-
-#endif //BUILD_MINIMALSUBSCRIBER_H

src/btree_nodes/src/nodes/WeightedRandomNode.cpp

@@ -25,7 +25,7 @@ NodeStatus WeightedRandomNode::tick() {
             return NodeStatus::FAILURE;
         }
 
-        std::regex rgx(";");
+        std::regex rgx(",");
         std::sregex_token_iterator iter(weightString.begin(),
                                         weightString.end(),
                                         rgx,

src/btree_trees/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(moveit_test)
+project(btree_trees)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -7,20 +7,11 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(moveit_ros_planning_interface REQUIRED)
-find_package(geometry_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-add_executable(move src/test.cpp)
+install(DIRECTORY   trees     DESTINATION   share/${PROJECT_NAME})
-set_property(TARGET move PROPERTY CXX_STANDARD 17)
-ament_target_dependencies(move rclcpp moveit_ros_planning_interface geometry_msgs)
-
-install(TARGETS
-        move
-        DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

src/btree_trees/package.xml

@@ -0,0 +1,13 @@
+<package format="3">
+  <name>btree_trees</name>
+  <version>0.244.1</version>
+  <description>Trees for the simulation</description>
+  <license>Apache 2.0</license>
+  <maintainer email="bastian.hofmann@xitaso.com">Bastian Hofmann</maintainer>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/btree_trees/trees/actorTree.xml

@@ -9,7 +9,7 @@
                 </Inverter>
 
                 <Sequence>
-                    <Control ID="WeightedRandom" weights="100;5;2">
+                    <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}"/>

src/btree_trees/trees/robotTree.xml

@@ -14,11 +14,11 @@
             <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="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
                         <Action ID="RobotMove" target="{target}"/>
                         <Action ID="RandomFailure" failureChance="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="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
                         <Action ID="RobotMove" target="{target}"/>
                         <Action ID="RandomFailure" failureChance="0.05"/>
                 </Control>

src/controller_test/CMakeLists.txt

@@ -1,36 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(controller_test)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(moveit_ros_planning_interface REQUIRED)
-find_package(control_msgs REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-add_executable(move src/test.cpp)
-set_property(TARGET move PROPERTY CXX_STANDARD 17)
-ament_target_dependencies(move rclcpp moveit_ros_planning_interface control_msgs)
-
-install(TARGETS
-        move
-        DESTINATION lib/${PROJECT_NAME})
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()

src/controller_test/package.xml

@@ -1,23 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>controller_test</name>
-  <version>0.1.0</version>
-  <description>
-     Test for Moveit on IISY
-  </description>
-  <author email="bastian.hofmann@xitaso.com">bastian</author>
-  <maintainer email="bastian.hofmann@xitaso.com">bastian</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>rclcpp</depend>
-  <depend>geometry_msgs</depend>
-  <depend>moveit_ros_planning_interface</depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-
-</package>

src/controller_test/src/test.cpp

@@ -1,196 +0,0 @@
-#include <memory>
-#include <string>
-#include <vector>
-
-#include <rclcpp/rclcpp.hpp>
-#include <rclcpp_action/rclcpp_action.hpp>
-
-#include <control_msgs/action/follow_joint_trajectory.hpp>
-
-std::shared_ptr<rclcpp::Node> node;
-bool common_goal_accepted = false;
-rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN;
-int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL;
-
-void common_goal_response(
-  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) {
-    common_goal_accepted = false;
-    printf("Goal rejected\n");
-  } else {
-    common_goal_accepted = true;
-    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());
-  common_resultcode = result.code;
-  common_action_result_code = result.result->error_code;
-  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(
-  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);
-
-  node = std::make_shared<rclcpp::Node>("trajectory_test_node");
-
-  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");
-
-  bool response =
-    action_client->wait_for_action_server(std::chrono::seconds(1));
-  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<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;
-
-  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;
-
-  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;
-
-  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] = 0.0;
-  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);
-
-  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
-  opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1);
-  opt.result_callback = std::bind(common_result_response, std::placeholders::_1);
-  opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2);
-
-  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;
-  }
-  RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
-
-  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;
-  
-  rclcpp::shutdown();
-
-  return 0;
-}

src/ign_world/launch/gazebo_controller_launch.py

@@ -53,6 +53,51 @@ def generate_launch_description():
             output='both',
         ),
         
+        Node(
+            package='btree_nodes',
+            executable='tree',
+            output='both',
+            parameters=[
+                {
+                    "trees": [
+                        get_package_share_directory('btree_trees')+'/trees/actorTree.xml',
+                        get_package_share_directory('btree_trees')+'/trees/robotTree.xml'
+                    ],
+                    "areas": [
+                        "safeArea",
+                        "1,   3.5 |"+
+                        "1,   7   |"+
+                        "3,   3.5 |"+
+                        "7,   7   |"+
+                        "3,  -1   |"+
+                        "7,  -1",
+                        "warningArea",
+                        "-1,  2.5 |"+
+                        "-1,  3.5 |"+
+                        "2,   2.5 |"+
+                        "3,   3.5 |"+
+                        "2,  -1   |"+
+                        "3,  -1",
+                        "unsafeArea",
+                        "-1,  1.5 |"+
+                        "-1,  2.5 |"+
+                        "1,   1.5 |"+
+                        "2,   2.5 |"+
+                        "1,  -1   |"+
+                        "2,  -1",
+                        "negativeYTable",
+                        "0.3,  -0.25 |"+
+                        "-0.3, -0.25 |"+
+                        "0,    -0.4",
+                        "positiveYTable",
+                        "0.3,   0.25 |"+
+                        "-0.3,  0.25 |"+
+                        "0,     0.4"
+                    ]
+                },
+            ]
+        ),
+
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
                 str(moveit_config.package_path / "launch/rsp.launch.py")

src/iisy_config/config/iisy.urdf

@@ -1,8 +1,4 @@
 <?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from src/iisy_config/urdf/iisy.urdf | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
 <robot name="iisy">
   <link name="world"/>
   <link name="base_link"/>

src/moveit_test/package.xml

@@ -1,23 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>moveit_test</name>
-  <version>0.1.0</version>
-  <description>
-     Test for Moveit on IISY
-  </description>
-  <author email="bastian.hofmann@xitaso.com">bastian</author>
-  <maintainer email="bastian.hofmann@xitaso.com">bastian</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>rclcpp</depend>
-  <depend>geometry_msgs</depend>
-  <depend>moveit_ros_planning_interface</depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-
-</package>

src/moveit_test/src/test.cpp

@@ -1,61 +0,0 @@
-#include <memory>
-#include <rclcpp/rclcpp.hpp>
-#include <moveit/move_group_interface/move_group_interface.h>
-
-int main(int argc, char * argv[])
-{
-    // Initialize ROS and create the Node
-    rclcpp::init(argc, argv);
-    auto const node = std::make_shared<rclcpp::Node>(
-        "hello_moveit",
-        rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
-        );
-
-    // Create a ROS logger
-    auto const logger = rclcpp::get_logger("hello_moveit");
-
-    // Create the MoveIt MoveGroup Interface
-    using moveit::planning_interface::MoveGroupInterface;
-    auto move_group_interface = MoveGroupInterface(node, "arm");
-
-    // Set a target Pose
-    auto const target_pose = []{
-        geometry_msgs::msg::Pose msg;
-        msg.orientation.w = 0.0;
-        msg.orientation.x = 0.0;
-        msg.orientation.y = 1.0;
-        msg.orientation.z = 0.0;
-        msg.position.x = -0.043357;
-        msg.position.y = -0.281366;
-        msg.position.z = 1.1;
-        return msg;
-    }();
-
-    move_group_interface.setGoalOrientationTolerance(0.01);
-    move_group_interface.setGoalPositionTolerance(0.01);
-    move_group_interface.setPoseTarget(target_pose);
-
-    //std::map<std::string,double> states = {
-    //    {"base_rot",10.0}
-    //};
-
-    //move_group_interface.setJointValueTarget(states);
-
-    // Create a plan to that target pose
-    auto const [success, plan] = [&move_group_interface]{
-        moveit::planning_interface::MoveGroupInterface::Plan msg;
-        auto const ok = static_cast<bool>(move_group_interface.plan(msg));
-        return std::make_pair(ok, msg);
-    }();
-
-    // Execute the plan
-    if(success) {
-        move_group_interface.execute(plan);
-    } else {
-        RCLCPP_ERROR(logger, "Planing failed!");
-    }
-
-    // Shutdown ROS
-    rclcpp::shutdown();
-    return 0;
-}