From ce51d7bf1b4b8a358e8f60ca4823c0fdfbe70141 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Mon, 18 Nov 2024 21:03:21 +0100 Subject: [PATCH] Initial Commit --- .gitignore | 3 + .gitmodules | 3 + .vscode/c_cpp_properties.json | 25 ++ .vscode/settings.json | 88 +++++++ ext/BehaviorTree.ROS2 | 1 + mg_bt/CMakeLists.txt | 68 ++++++ mg_bt/behavior_tree_nodes/nodes.xml | 16 ++ mg_bt/behavior_trees/mg_bt.btproj | 19 ++ mg_bt/behavior_trees/untitled_1.xml | 36 +++ mg_bt/config/mg_bt_executor_config.yaml | 14 ++ mg_bt/include/mg_bt/MoveStraightBehavior.hpp | 30 +++ mg_bt/launch/mg_bt_launch.py | 25 ++ mg_bt/package.xml | 24 ++ mg_bt/src/MoveStraightBehavior.cpp | 26 ++ mg_bt/src/mg_bt_executor.cpp | 95 ++++++++ mg_msgs/CMakeLists.txt | 30 +++ mg_msgs/action/MoveStraight.action | 12 + mg_msgs/msg/Point2d.msg | 2 + mg_msgs/package.xml | 22 ++ mg_navigation/CMakeLists.txt | 61 +++++ mg_navigation/package.xml | 27 +++ mg_navigation/src/mg_move_server.cpp | 237 +++++++++++++++++++ 22 files changed, 864 insertions(+) create mode 100644 .gitignore create mode 100644 .gitmodules create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/settings.json create mode 160000 ext/BehaviorTree.ROS2 create mode 100644 mg_bt/CMakeLists.txt create mode 100644 mg_bt/behavior_tree_nodes/nodes.xml create mode 100644 mg_bt/behavior_trees/mg_bt.btproj create mode 100644 mg_bt/behavior_trees/untitled_1.xml create mode 100644 mg_bt/config/mg_bt_executor_config.yaml create mode 100644 mg_bt/include/mg_bt/MoveStraightBehavior.hpp create mode 100644 mg_bt/launch/mg_bt_launch.py create mode 100644 mg_bt/package.xml create mode 100644 mg_bt/src/MoveStraightBehavior.cpp create mode 100644 mg_bt/src/mg_bt_executor.cpp create mode 100644 mg_msgs/CMakeLists.txt create mode 100644 mg_msgs/action/MoveStraight.action create mode 100644 mg_msgs/msg/Point2d.msg create mode 100644 mg_msgs/package.xml create mode 100644 mg_navigation/CMakeLists.txt create mode 100644 mg_navigation/package.xml create mode 100644 mg_navigation/src/mg_move_server.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..29cad58 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +build/ +install/ +log/ \ No newline at end of file diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..4c994e1 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "ext/BehaviorTree.ROS2"] + path = ext/BehaviorTree.ROS2 + url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..01ed181 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,25 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": true + }, + "includePath": [ + "/opt/ros/jazzy/include/**", + "${workspaceFolder}/ext/**/src", + "${workspaceFolder}/ext/**/**/**", + "${workspaceFolder}/install/btcpp_ros2_interfaces/include/btcpp_ros2_interfaces", + "${workspaceFolder}/install/mg_msgs/include/mg_msgs", + "/usr/include/glm", + "${workspaceFolder}/mg_bt/include" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/g++", + "cStandard": "gnu11", + "cppStandard": "c++17" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..c408018 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,88 @@ +{ + "files.associations": { + "*.embeddedhtml": "html", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "cfenv": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "expected": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "source_location": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "format": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "ranges": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "span": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stdfloat": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "valarray": "cpp", + "variant": "cpp" + }, + "cmake.ignoreCMakeListsMissing": true +} \ No newline at end of file diff --git a/ext/BehaviorTree.ROS2 b/ext/BehaviorTree.ROS2 new file mode 160000 index 0000000..cc31ea7 --- /dev/null +++ b/ext/BehaviorTree.ROS2 @@ -0,0 +1 @@ +Subproject commit cc31ea7b97947f1aac6e8c37df6cec379c84a7d9 diff --git a/mg_bt/CMakeLists.txt b/mg_bt/CMakeLists.txt new file mode 100644 index 0000000..1613fde --- /dev/null +++ b/mg_bt/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.8) +project(mg_bt) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + + +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(behaviortree_cpp REQUIRED) +find_package(behaviortree_ros2 REQUIRED) +find_package(btcpp_ros2_interfaces REQUIRED) +find_package(mg_msgs REQUIRED) + +set(PACKAGE_DEPS + rclcpp + behaviortree_cpp + behaviortree_ros2 + btcpp_ros2_interfaces + mg_msgs +) + + +add_executable(mg_bt_executor + src/mg_bt_executor.cpp + src/MoveStraightBehavior.cpp) +ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS}) + +target_include_directories( + mg_bt_executor + PRIVATE + include +) + +install( TARGETS + mg_bt_executor + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + behavior_tree_nodes + behavior_trees + config + launch + DESTINATION share/${PROJECT_NAME}/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + + +ament_export_dependencies(behaviortree_ros2 btcpp_ros2_interfaces) + +ament_package() diff --git a/mg_bt/behavior_tree_nodes/nodes.xml b/mg_bt/behavior_tree_nodes/nodes.xml new file mode 100644 index 0000000..6e1f610 --- /dev/null +++ b/mg_bt/behavior_tree_nodes/nodes.xml @@ -0,0 +1,16 @@ + + + + + + Action server name + + + + + + + Action server name + + + \ No newline at end of file diff --git a/mg_bt/behavior_trees/mg_bt.btproj b/mg_bt/behavior_trees/mg_bt.btproj new file mode 100644 index 0000000..e0bcc22 --- /dev/null +++ b/mg_bt/behavior_trees/mg_bt.btproj @@ -0,0 +1,19 @@ + + + + + + + + + Action server name + + + + + + + Action server name + + + diff --git a/mg_bt/behavior_trees/untitled_1.xml b/mg_bt/behavior_trees/untitled_1.xml new file mode 100644 index 0000000..fcb3e3f --- /dev/null +++ b/mg_bt/behavior_trees/untitled_1.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + Action server name + + + + + + + diff --git a/mg_bt/config/mg_bt_executor_config.yaml b/mg_bt/config/mg_bt_executor_config.yaml new file mode 100644 index 0000000..c4c546f --- /dev/null +++ b/mg_bt/config/mg_bt_executor_config.yaml @@ -0,0 +1,14 @@ +bt_action_server: + ros__parameters: + action_name: "mg_bt_action_server" # Optional (defaults to `bt_action_server`) + tick_frequency: 100 # Optional (defaults to 100 Hz) + groot2_port: 8420 # Optional (defaults to 1667) + ros_plugins_timeout: 1000 # Optional (defaults 1000 ms) + + plugins: + # - behaviortree_cpp/bt_plugins + - btcpp_ros2_samples/bt_plugins + + behavior_trees: + - mg_bt/behavior_trees + diff --git a/mg_bt/include/mg_bt/MoveStraightBehavior.hpp b/mg_bt/include/mg_bt/MoveStraightBehavior.hpp new file mode 100644 index 0000000..f42726c --- /dev/null +++ b/mg_bt/include/mg_bt/MoveStraightBehavior.hpp @@ -0,0 +1,30 @@ +#include "behaviortree_ros2/bt_action_node.hpp" +#include "mg_msgs/action/move_straight.hpp" +#include "rclcpp/rclcpp.hpp" + + + +class MoveStraightAction : public BT::RosActionNode +{ +public: + MoveStraightAction(const std::string& name, const BT::NodeConfig& conf, + const BT::RosNodeParams& params) + : BT::RosActionNode(name, conf, params) + {} + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("distance"), + BT::InputPort("tolerance", 0.1, {}) + }); + } + + bool setGoal(Goal& goal) override; + + void onHalt() override; + + BT::NodeStatus onResultReceived(const WrappedResult& wr) override; + + virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override; +}; \ No newline at end of file diff --git a/mg_bt/launch/mg_bt_launch.py b/mg_bt/launch/mg_bt_launch.py new file mode 100644 index 0000000..e2a2914 --- /dev/null +++ b/mg_bt/launch/mg_bt_launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction +from launch.event_handlers import OnProcessStart +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from pathlib import Path + +def generate_launch_description(): + + basedir = FindPackageShare("mg_bt") + + yamlBT = PathJoinSubstitution([basedir, "config/mg_bt_executor_config.yaml"]) + + return LaunchDescription([ + Node( + package='mg_bt', + executable='mg_bt_executor', + output="screen", + parameters=[yamlBT] + ), + ]) + + diff --git a/mg_bt/package.xml b/mg_bt/package.xml new file mode 100644 index 0000000..3eaf6a1 --- /dev/null +++ b/mg_bt/package.xml @@ -0,0 +1,24 @@ + + + + mg_bt + 0.0.0 + TODO: Package description + petar + TODO: License declaration + + ament_cmake + + rclcpp + behaviortree_cpp + behaviortree_ros2 + btcpp_ros2_interfaces + mg_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mg_bt/src/MoveStraightBehavior.cpp b/mg_bt/src/MoveStraightBehavior.cpp new file mode 100644 index 0000000..53b5526 --- /dev/null +++ b/mg_bt/src/MoveStraightBehavior.cpp @@ -0,0 +1,26 @@ +#include "mg_bt/MoveStraightBehavior.hpp" + + +bool MoveStraightAction::setGoal(Goal& goal) { + auto dist = getInput("distance"); + auto tolerance = getInput("tolerance"); + goal.distance = dist.value(); + goal.tolerance = tolerance.value(); + return true; +} + +BT::NodeStatus MoveStraightAction::onResultReceived(const RosActionNode::WrappedResult& wr) +{ + return (!wr.result->error) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; +} + +BT::NodeStatus MoveStraightAction::onFailure(BT::ActionNodeErrorCode error) +{ + RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error)); + return BT::NodeStatus::FAILURE; +} + +void MoveStraightAction::onHalt() +{ + RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); +} \ No newline at end of file diff --git a/mg_bt/src/mg_bt_executor.cpp b/mg_bt/src/mg_bt_executor.cpp new file mode 100644 index 0000000..eb56876 --- /dev/null +++ b/mg_bt/src/mg_bt_executor.cpp @@ -0,0 +1,95 @@ +#include +#include +#include +#include + + +#include +#include + + +#include "rclcpp/rclcpp.hpp" + +#include "mg_bt/MoveStraightBehavior.hpp" + +class MgSimpleBehavior : public BT::SyncActionNode { +public: + MgSimpleBehavior( const std::string& name, const BT::NodeConfig& config ) + : BT::SyncActionNode(name, config) { + } + + static BT::PortsList providedPorts() + { + return { BT::InputPort("message") }; + } + BT::NodeStatus tick() override{ + + std::cout << "This is an example action called " << this->name() << std::endl; + + auto msg = getInput("message"); + + if(!msg) { + std::cout << "message was not provided" << std::endl; + } + else { + std::cout << "The message is " << msg.value() << std::endl; + } + + return BT::NodeStatus::SUCCESS; + } + +}; + + +class MgBtExecutionServer : public BT::TreeExecutionServer { +public: + MgBtExecutionServer(const rclcpp::NodeOptions options) + : TreeExecutionServer(options) { + executeRegistration(); + std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl; + } + + void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override { + factory.registerNodeType("SimpleBehavior"); + factory.registerNodeType("MoveStraightAction", node()); + + } + + void onTreeCreated(BT::Tree& tree) override { + logger_cout_ = std::make_shared(tree); + } + + std::optional + onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override { + (void)status; + logger_cout_.reset(); + + if (was_cancelled) { + std::cout << "The tree execution was cancled" << std::endl; + } + + return std::nullopt; + } + +private: + std::shared_ptr logger_cout_; +}; + +int main(int argc, const char** argv) { + rclcpp::init(argc,argv); + + + rclcpp::NodeOptions options; + auto bt_server = std::make_shared(options); + + + rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250)); + + + exec.add_node(bt_server->node()); + exec.spin(); + exec.remove_node(bt_server->node()); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/mg_msgs/CMakeLists.txt b/mg_msgs/CMakeLists.txt new file mode 100644 index 0000000..f4759bd --- /dev/null +++ b/mg_msgs/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(mg_msgs) + +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(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Point2d.msg" + "action/MoveStraight.action" +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/mg_msgs/action/MoveStraight.action b/mg_msgs/action/MoveStraight.action new file mode 100644 index 0000000..c0eb11d --- /dev/null +++ b/mg_msgs/action/MoveStraight.action @@ -0,0 +1,12 @@ +float64 distance +float64 tolerance 0.005 +string[] ignore_tags [] +--- +uint8 SUCCESS=0 +uint8 BLOCKED=1 +uint8 TIMEOUT=2 +uint8 UNKNOWN=254 + +uint8 error +--- +float64 distance_passed \ No newline at end of file diff --git a/mg_msgs/msg/Point2d.msg b/mg_msgs/msg/Point2d.msg new file mode 100644 index 0000000..125c004 --- /dev/null +++ b/mg_msgs/msg/Point2d.msg @@ -0,0 +1,2 @@ +float64 x +float64 y \ No newline at end of file diff --git a/mg_msgs/package.xml b/mg_msgs/package.xml new file mode 100644 index 0000000..506f553 --- /dev/null +++ b/mg_msgs/package.xml @@ -0,0 +1,22 @@ + + + + mg_msgs + 0.0.0 + TODO: Package description + petar + TODO: License declaration + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mg_navigation/CMakeLists.txt b/mg_navigation/CMakeLists.txt new file mode 100644 index 0000000..84e4ae7 --- /dev/null +++ b/mg_navigation/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(mg_navigation) + +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(mg_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +include(FindPkgConfig) +pkg_search_module(LIBGLM REQUIRED glm) + + + +set(PACKAGE_DEPS + rclcpp + mg_msgs + geometry_msgs + tf2 + tf2_ros +) + +add_executable(mg_move_server src/mg_move_server.cpp) +ament_target_dependencies(mg_move_server ${PACKAGE_DEPS}) + +target_include_directories( + mg_move_server + PRIVATE + ${LIBGLM_INCLUDE_DIRS} + include +) + +target_link_libraries( + mg_move_server + ${LIBGLM_LIBRARIES} +) + +install( TARGETS + mg_move_server + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/mg_navigation/package.xml b/mg_navigation/package.xml new file mode 100644 index 0000000..5be314a --- /dev/null +++ b/mg_navigation/package.xml @@ -0,0 +1,27 @@ + + + + mg_navigation + 0.0.0 + TODO: Package description + petar + TODO: License declaration + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_components + mg_msgs + geometry_msgs + tf2 + tf2_ros + libglm-dev + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mg_navigation/src/mg_move_server.cpp b/mg_navigation/src/mg_move_server.cpp new file mode 100644 index 0000000..f985de6 --- /dev/null +++ b/mg_navigation/src/mg_move_server.cpp @@ -0,0 +1,237 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + + + +#include "mg_msgs/action/move_straight.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" + +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Vector3.h" +#include "tf2/LinearMath/Quaternion.h" + +namespace mg { + +namespace Geometry = geometry_msgs::msg; + + +class MoveServer : public rclcpp::Node { +public: + using MoveAction = mg_msgs::action::MoveStraight; + using MoveGoalHandle = rclcpp_action::ServerGoalHandle; + + + MoveServer(const rclcpp::NodeOptions options) + : rclcpp::Node("MgMoveServer",options) + { + + std::cout << "something works?" << std::endl; + tf2_buffer_ = std::make_shared(get_clock()); + tf2_listener_ = std::make_shared(*tf2_buffer_); + using namespace std::placeholders; + command_publisher_ = create_publisher("diffdrive_controller/cmd_vel", 2); + + move_action_server_ = rclcpp_action::create_server( + this, + "MoveStraight", + std::bind(&MoveServer::handle_goal, this, _1, _2), + std::bind(&MoveServer::handle_cancel, this, _1), + std::bind(&MoveServer::handle_accepted, this, _1) + ); + RCLCPP_INFO(this->get_logger(), "Registered Server"); + } + +private: + + + std::shared_ptr > move_action_server_; + std::shared_ptr > command_publisher_; + + + std::shared_ptr > tf2_subscription_; + std::shared_ptr tf2_buffer_; + std::shared_ptr tf2_listener_; + std::mutex point_mtx_; + glm::dvec2 point2d_; + double theta_; + + std::mutex processing_mtx_; + bool is_processing_goal_ = false; + + void process_tf2() { + + + if(tf2_buffer_->canTransform("odom", "base-link", rclcpp::Time(0))) { + + auto tf2_frame = tf2_buffer_->lookupTransform("odom", "base-link", rclcpp::Time(0)).transform; + + tf2::Quaternion quaternion; + quaternion.setW(tf2_frame.rotation.w); + quaternion.setX(tf2_frame.rotation.x); + quaternion.setY(tf2_frame.rotation.y); + quaternion.setZ(tf2_frame.rotation.z); + + double theta,ro,phi; + + tf2::Matrix3x3(quaternion).getRPY(ro,phi,theta); + point_mtx_.lock(); + point2d_ = {tf2_frame.translation.x,tf2_frame.translation.y}; + + point_mtx_.unlock(); + + RCLCPP_INFO(this->get_logger(), "Got a tf2: x = %lf; y = %lf; theta = %lf", point2d_.x, point2d_.y, theta_); + } + } + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr + ) { + RCLCPP_INFO(this->get_logger(), "RECIEVED GOAL"); + (void)uuid; + + std::lock_guard lock(processing_mtx_); + + if(is_processing_goal_) { + return rclcpp_action::GoalResponse::REJECT; + } else { + is_processing_goal_ = true; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + } + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle + ) { + (void)goal_handle; + RCLCPP_INFO(this->get_logger(), "We will now canel the move request"); + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(const std::shared_ptr goal_handle) { + RCLCPP_INFO(this->get_logger(),"GOAL_ACCEPTED"); + using namespace std::placeholders; + auto t = std::bind(&MoveServer::execute_move, this, _1); + std::thread{t,goal_handle}.detach(); + } + + Geometry::TwistStamped to_twist(double v, double w) { + auto twist = Geometry::TwistStamped(); + twist.twist.angular.z = w; + twist.twist.linear.x = v; + return twist; + } + + void execute_move(const std::shared_ptr goal_handle) { + + auto goal = goal_handle->get_goal(); + + double target_distance = goal->distance; + double tolerance = goal->tolerance; + + double increments = 0.01; + + glm::dvec2 c_point, t_point; + process_tf2(); + double c_theta; + { + std::lock_guard _lock(point_mtx_); + c_point = point2d_; + c_theta = theta_; + } + + glm::dmat2 rotmat = glm::dmat2(glm::rotate(glm::dmat4(1), c_theta, glm::dvec3(0,0,1))); + + RCLCPP_INFO(this->get_logger(), "Rotmat: %lf;%lf;%lf;%lf;", rotmat[0].x,rotmat[0].y,rotmat[1].x,rotmat[1].y); + glm::dvec2 dir(1,0); + + dir = rotmat * dir; + + t_point = c_point + dir*target_distance; + + RCLCPP_INFO(this->get_logger(), "Target set to %lf;%lf", t_point.x,t_point.y); + int speed_multi=0; + + rclcpp::Rate rate(100); + while(glm::length2(c_point-t_point) >= tolerance * tolerance + && rclcpp::ok()){ + + if(goal_handle->is_canceling()) { + auto x = std::make_shared(); + x->error = MoveAction::Result::UNKNOWN; + command_publisher_->publish(to_twist(0,0)); + is_processing_goal_ = false; + goal_handle->canceled(x); + return; + } + + double best_score = -INFINITY; + int i_b = 0; + for(int i = -2; i<3 ;i++){ + double lookahead = glm::abs(speed_multi+i); + glm::dvec2 d_point = c_point + dir * ((increments * (speed_multi+i))*lookahead/100.0); + + RCLCPP_INFO(this->get_logger(), "The lookahead is: %lf. I will be multiplying that with %lf", lookahead, (increments * (speed_multi+i))); + RCLCPP_INFO(this->get_logger(), "The estimated position is x: %lf y: %lf", d_point.x, d_point.y); + RCLCPP_INFO(this->get_logger(), "The score is %lf",score(d_point, c_point, t_point) ); + + if(score(d_point, c_point, t_point) > best_score) { + best_score = score(d_point, c_point, t_point); + i_b = i; + } + } + + speed_multi += i_b; + + command_publisher_->publish(to_twist(increments * speed_multi,0.0)); + + RCLCPP_INFO(this->get_logger(), "Would've sent the following %lf", increments * speed_multi); + + + + process_tf2(); + { + std::lock_guard _lock(point_mtx_); + c_point = point2d_; + c_theta = theta_; + } + + rate.sleep(); + } + if(rclcpp::ok()) { + auto x = std::make_shared(); + x->error = MoveAction::Result::SUCCESS; + is_processing_goal_ = false; + goal_handle->succeed(x); + } + } + + double score(glm::dvec2 point, glm::dvec2 c_point, glm::dvec2 t_point) { + return glm::length2(t_point-c_point) - glm::length2(t_point-point); + } +}; + + +}; + +int main(int argc, const char** argv) { + rclcpp::init(argc,argv); + std::cout << "something works?" << std::endl; + rclcpp::NodeOptions options; + auto mg_move_server = std::make_shared(options); + rclcpp::spin(mg_move_server); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file