Initial Commit
This commit is contained in:
237
mg_navigation/src/mg_move_server.cpp
Normal file
237
mg_navigation/src/mg_move_server.cpp
Normal 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;
|
||||
}
|
||||
Reference in New Issue
Block a user