Pimpest/ros2-behaviors
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial Commit

Browse Source
This commit is contained in:
Pimpest
2024-11-18 21:03:21 +01:00
commit ce51d7bf1b
22 changed files with 864 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored Normal file
View File

@ -0,0 +1,3 @@
build/
install/
log/

3
.gitmodules vendored Normal file
View File

@ -0,0 +1,3 @@
[submodule "ext/BehaviorTree.ROS2"]
path = ext/BehaviorTree.ROS2
url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git

25
.vscode/c_cpp_properties.json vendored Normal file
View File

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

88
.vscode/settings.json vendored Normal file
View File

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

1
ext/BehaviorTree.ROS2 Submodule

Submodule ext/BehaviorTree.ROS2 added at cc31ea7b97

68
mg_bt/CMakeLists.txt Normal file
View File

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

16
mg_bt/behavior_tree_nodes/nodes.xml Normal file
View File

@ -0,0 +1,16 @@
<root BTCPP_format="4">
<TreeNodesModel>
<Action ID="MoveStraightAction">
<input_port name="distance" type="double"/>
<input_port name="tolerance" type="double" default="0.100000"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
</Action>
<Action ID="SimpleBehavior">
<input_port name="message" type="std::string"/>
</Action>
<Action ID="SleepAction">
<input_port name="msec" type="unsigned int"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
</Action>
</TreeNodesModel>
</root>

19
mg_bt/behavior_trees/mg_bt.btproj Normal file
View File

@ -0,0 +1,19 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4" project_name="Testing">
<include path="untitled_1.xml"/>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="MoveStraightAction">
<input_port name="distance" type="double"/>
<input_port name="tolerance" default="0.100000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="SimpleBehavior">
<input_port name="message" type="std::string"/>
</Action>
<Action ID="SleepAction">
<input_port name="msec" type="unsigned int"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
</TreeNodesModel>
</root>

36
mg_bt/behavior_trees/untitled_1.xml Normal file
View File

@ -0,0 +1,36 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4"
main_tree_to_execute="Main_strat">
<BehaviorTree ID="Main_strat">
<Repeat num_cycles="25">
<Sequence>
<MoveStraightAction distance="5"
tolerance="0.001"
action_name="/MoveStraight"/>
<Sleep msec="1000"/>
<SimpleBehavior message="YoYoYo Mr. White, what up?"/>
<MoveStraightAction distance="-5"
tolerance="0.001"
action_name="/MoveStraight"/>
</Sequence>
</Repeat>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="MoveStraightAction">
<input_port name="distance"
type="double"/>
<input_port name="tolerance"
default="0.100000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="SimpleBehavior">
<input_port name="message"
type="std::string"/>
</Action>
</TreeNodesModel>
</root>

14
mg_bt/config/mg_bt_executor_config.yaml Normal file
View File

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

30
mg_bt/include/mg_bt/MoveStraightBehavior.hpp Normal file
View File

@ -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<mg_msgs::action::MoveStraight>
{
public:
MoveStraightAction(const std::string& name, const BT::NodeConfig& conf,
const BT::RosNodeParams& params)
: BT::RosActionNode<mg_msgs::action::MoveStraight>(name, conf, params)
{}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<double>("distance"),
BT::InputPort<double>("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;
};

25
mg_bt/launch/mg_bt_launch.py Normal file
View File

@ -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]
),
])

24
mg_bt/package.xml Normal file
View File

@ -0,0 +1,24 @@
<?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>mg_bt</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="82343504+Pimpest@users.noreply.github.com">petar</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>behaviortree_cpp</depend>
<depend>behaviortree_ros2</depend>
<depend>btcpp_ros2_interfaces</depend>
<depend>mg_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

26
mg_bt/src/MoveStraightBehavior.cpp Normal file
View File

@ -0,0 +1,26 @@
#include "mg_bt/MoveStraightBehavior.hpp"
bool MoveStraightAction::setGoal(Goal& goal) {
auto dist = getInput<double>("distance");
auto tolerance = getInput<double>("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());
}

95
mg_bt/src/mg_bt_executor.cpp Normal file
View File

@ -0,0 +1,95 @@
#include <behaviortree_ros2/tree_execution_server.hpp>
#include <behaviortree_cpp/loggers/bt_cout_logger.h>
#include <behaviortree_cpp/xml_parsing.h>
#include <behaviortree_cpp/bt_factory.h>
#include <memory>
#include <iostream>
#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<std::string>("message") };
}
BT::NodeStatus tick() override{
std::cout << "This is an example action called " << this->name() << std::endl;
auto msg = getInput<std::string>("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<MgSimpleBehavior>("SimpleBehavior");
factory.registerNodeType<MoveStraightAction>("MoveStraightAction", node());
}
void onTreeCreated(BT::Tree& tree) override {
logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
}
std::optional<std::string>
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<BT::StdCoutLogger> logger_cout_;
};
int main(int argc, const char** argv) {
rclcpp::init(argc,argv);
rclcpp::NodeOptions options;
auto bt_server = std::make_shared<MgBtExecutionServer>(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;
}

30
mg_msgs/CMakeLists.txt Normal file
View File

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

12
mg_msgs/action/MoveStraight.action Normal file
View File

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

2
mg_msgs/msg/Point2d.msg Normal file
View File

@ -0,0 +1,2 @@
float64 x
float64 y

22
mg_msgs/package.xml Normal file
View File

@ -0,0 +1,22 @@
<?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>mg_msgs</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="82343504+Pimpest@users.noreply.github.com">petar</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

61
mg_navigation/CMakeLists.txt Normal file
View File

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

27
mg_navigation/package.xml Normal file
View File

@ -0,0 +1,27 @@
<?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>mg_navigation</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="82343504+Pimpest@users.noreply.github.com">petar</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>rclcpp_components</depend>
<depend>mg_msgs</depend>
<depend>geometry_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<build_depend>libglm-dev</build_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

237
mg_navigation/src/mg_move_server.cpp Normal file
View File

@ -0,0 +1,237 @@
#include <functional>
#include <memory>
#include <thread>
#include <string>
#include <cstring>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtx/norm.hpp>
#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<MoveAction>;
MoveServer(const rclcpp::NodeOptions options)
: rclcpp::Node("MgMoveServer",options)
{
std::cout << "something works?" << std::endl;
tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
using namespace std::placeholders;
command_publisher_ = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);
move_action_server_ = rclcpp_action::create_server<MoveAction>(
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<rclcpp_action::Server<MoveAction> > move_action_server_;
std::shared_ptr<rclcpp::Publisher<Geometry::TwistStamped> > command_publisher_;
std::shared_ptr<rclcpp::Subscription<Geometry::TransformStamped> > tf2_subscription_;
std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
std::shared_ptr<tf2_ros::TransformListener> 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<const MoveAction::Goal>
) {
RCLCPP_INFO(this->get_logger(), "RECIEVED GOAL");
(void)uuid;
std::lock_guard<std::mutex> 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<MoveGoalHandle> 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<MoveGoalHandle> 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<MoveGoalHandle> 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<MoveAction::Result>();
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<MoveAction::Result>();
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<mg::MoveServer>(options);
rclcpp::spin(mg_move_server);
rclcpp::shutdown();
return 0;
}