From 9141355f4b2a44b5d0b5e5d29f78ed91fe8b22b2 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Sun, 7 Nov 2021 23:27:03 +0100
Subject: [PATCH] Solved a bug when computing the footprint collision: One of
the variables was cast to an integer and the Infinity value has been changed
by the maximum double value. Solved a bug when the linear speed was 0.0: The
trajectory was computed backwards in this case.
---
src/safe_cmd_alg.cpp | 2 +-
src/safe_cmd_alg_node.cpp | 6 +++---
2 files changed, 4 insertions(+), 4 deletions(-)
diff --git a/src/safe_cmd_alg.cpp b/src/safe_cmd_alg.cpp
index bbe9028..0a44df6 100644
--- a/src/safe_cmd_alg.cpp
+++ b/src/safe_cmd_alg.cpp
@@ -43,7 +43,7 @@ void SafeCmdAlgorithm::compute_trajectory_ackermann_steer_angle(double linear_x,
{
if(radius!=std::numeric_limits<double>::max())
{
- if(linear_x>0.0)
+ if(linear_x>=0.0)
traj[i].theta=traj[i-1].theta+this->config_.traj_sim_step/radius;
else
traj[i].theta=traj[i-1].theta-this->config_.traj_sim_step/radius;
diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp
index 94cdf2e..a26f889 100644
--- a/src/safe_cmd_alg_node.cpp
+++ b/src/safe_cmd_alg_node.cpp
@@ -116,7 +116,7 @@ void SafeCmdAlgNode::mainNodeThread(void)
if(collision_length>this->config.safety_distance)
break;
}
- }
+ }
if(collision_length>=this->config.safety_distance)
{
if(this->config.architecture==0)
@@ -284,7 +284,7 @@ bool SafeCmdAlgNode::on_segment(geometry_msgs::Point &p,geometry_msgs::Point &q,
int SafeCmdAlgNode::orientation(geometry_msgs::Point &p,geometry_msgs::Point &q,geometry_msgs::Point &r)
{
- int val = (q.y - p.y) * (r.x - q.x) -
+ double val = (q.y - p.y) * (r.x - q.x) -
(q.x - p.x) * (r.y - q.y);
if (val == 0) return 0;
@@ -315,7 +315,7 @@ bool SafeCmdAlgNode::inside(std::vector<geometry_msgs::Point> &footprint,pcl::Po
int count=0,i=0;
geometry_msgs::Point extreme,p;
- extreme.x=std::numeric_limits<double>::infinity();
+ extreme.x=std::numeric_limits<double>::max();
extreme.y=point.y;
p.x=point.x;
p.y=point.y;
--
GitLab