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

Added aborting if robot gets misaligned

Browse Source
This commit is contained in:
Pimpest
2024-12-08 14:34:52 +01:00
parent 0c33502b90
commit 659eead34d
2 changed files with 21 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
mg_msgs/action/MoveStraight.action
View File

@ -5,6 +5,7 @@ string[] ignore_tags []
uint8 SUCCESS=0 uint8 SUCCESS=0
uint8 BLOCKED=1 uint8 BLOCKED=1
uint8 TIMEOUT=2 uint8 TIMEOUT=2
uint8 MISALIGNED=3
uint8 UNKNOWN=254 uint8 UNKNOWN=254
uint8 error uint8 error

20
mg_navigation/src/mg_move_server.cpp
View File

@ -135,6 +135,14 @@ private:
return twist; return twist;
} }
double CalcMiss(glm::dvec2 a, glm::dvec2 b, glm::dvec2 c, double theta) {
glm::dvec2 v1 = a-b;
glm::dvec2 v2 = c-b;
double prod = glm::clamp(glm::dot(v1,v2)/(glm::length(v1) * glm::length(v2)), -1.0, 1.0);
double phi = glm::acos(prod);
return glm::sin(phi+theta) * -glm::length(v2);
}
void execute_move(const std::shared_ptr<MoveGoalHandle> goal_handle) { void execute_move(const std::shared_ptr<MoveGoalHandle> goal_handle) {
auto goal = goal_handle->get_goal(); auto goal = goal_handle->get_goal();
@ -153,12 +161,16 @@ private:
c_theta = theta_; c_theta = theta_;
} }
double og_theta = c_theta;
glm::dvec2 og_point = c_point;
glm::dmat2 rotmat = glm::dmat2(glm::rotate(glm::dmat4(1), c_theta, glm::dvec3(0,0,1))); 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); 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); glm::dvec2 dir(1,0);
dir = rotmat * dir; dir = rotmat * dir;
t_point = c_point + dir*target_distance; t_point = c_point + dir*target_distance;
@ -178,6 +190,14 @@ private:
return; return;
} }
if(CalcMiss(og_point, t_point, c_point, c_theta-og_theta) > tolerance) {
command_publisher_->publish(to_twist(0.0 ,0.0));
auto x = std::make_shared<MoveAction::Result>();
x->error = MoveAction::Result::MISALIGNED;
is_processing_goal_ = false;
goal_handle->abort(x);
}
double best_score = -INFINITY; double best_score = -INFINITY;
int i_b = 0; int i_b = 0;
for(int i = -2; i<3 ;i++){ for(int i = -2; i<3 ;i++){