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

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