void HapticsPSM::compute_average_normal(std::vector<tf::Vector3> &v_arr, tf::Vector3 &v){
tf::Vector3 new_n;
new_n.setValue(0,0,0);
if(v_arr.size() == 1){
new_n = v_arr.at(0);
}
else{
for(size_t i=0 ; i < v_arr.size() ; i++){
new_n = new_n + v_arr.at(i);
}
}
new_n.normalize();
if(v.length() == 0){
v = new_n; // This is for the first contact, since the current normal is zero, so assign the new value of the normal computed form collision checking
}
else{
//Check if new_n = 0 or in the negative direction as curr n, if it is, leave n unchanged
if(v.dot(new_n) == 0){ // If the dot product of the last and the current normal in 0, this could be due to the bug of getting negative normals
//v remains unchanged
ROS_INFO("New Normal Cancels out the previous one");
}
else if(v.dot(new_n) < 0 ){ //If the dot product of the last and the current normal in a negative number, this could be due to the bug of getting negative normals
v = -new_n;
ROS_INFO("New normal lies on the opposite plane");
}
else{
v = new_n;
}
}
}
void HapticsPSM::compute_deflection_along_normal(tf::Vector3 &n, tf::Vector3 &d, tf::Vector3 &d_along_n){
d_along_n = (d.dot(n)/n.length()) * n; //This gives us the dot product of current deflection in the direction of current normal
}
tf::Vector3 sat(const tf::Vector3 v, double val)
{
return v*val/std::max(val,v.length());
}
void speedsForObstacles(
mnm::RouteSpeedArray &speeds,
std::vector<DistanceReport> &reports,
const Route &route,
const mnm::RoutePosition &route_position,
const std::vector<ObstacleData> &obstacles,
const SpeedForObstaclesParameters &p)
{
tf::Vector3 local_fl(p.origin_to_left_m_, p.origin_to_left_m_, 0.0);
tf::Vector3 local_fr(p.origin_to_left_m_, -p.origin_to_right_m_, 0.0);
tf::Vector3 local_br(-p.origin_to_right_m_, -p.origin_to_right_m_, 0.0);
tf::Vector3 local_bl(-p.origin_to_right_m_, p.origin_to_left_m_, 0.0);
double car_r = 0.0;
car_r = std::max(car_r, local_fl.length());
car_r = std::max(car_r, local_fr.length());
car_r = std::max(car_r, local_br.length());
car_r = std::max(car_r, local_bl.length());
bool skip_point = true;
for (size_t route_index = 0; route_index < route.points.size(); route_index++) {
const RoutePoint &point = route.points[route_index];
if (skip_point) {
if (point.id() == route_position.id) {
skip_point = false;
}
continue;
}
// Use half of the vehicle width override as the car radius when smaller
// This is used for tight routes that are known to be safe such as when going
// though cones. It prevents objects close to the vehicle from causing the vehicle
// to stop/slow down too much unnecessarily.
double veh_r = car_r;
if (point.hasProperty("vehicle_width_override"))
{
ROS_DEBUG("Speeds for obstacle found vehicle_width_override property");
double width = point.getTypedProperty<double>("vehicle_width_override");
// Pick the smaller of the radii
if (veh_r >= width/2.0)
{
veh_r = width/2.0;
ROS_WARN_THROTTLE(1.0, "Vehicle width being overriden to %0.2f", (float)veh_r);
}
}
for (const auto& obstacle: obstacles) {
const tf::Vector3 v = obstacle.center - point.position();
const double d = v.length() - veh_r - obstacle.radius;
if (d > p.max_distance_m_) {
// The obstacle is too far away from this point to be a concern
continue;
}
tf::Vector3 closest_point = obstacle.center;
double distance = std::numeric_limits<double>::max();
for (size_t i = 1; i < obstacle.polygon.size(); i++) {
double dist = swri_geometry_util::DistanceFromLineSegment(
obstacle.polygon[i - 1],
obstacle.polygon[i],
point.position()) - veh_r;
if (dist < distance) {
distance = dist;
closest_point = swri_geometry_util::ProjectToLineSegment(
obstacle.polygon[i - 1],
obstacle.polygon[i],
point.position());
}
}
if (obstacle.polygon.size() > 1) {
double dist = swri_geometry_util::DistanceFromLineSegment(
obstacle.polygon.back(),
obstacle.polygon.front(),
point.position()) - veh_r;
if (dist < distance) {
distance = dist;
closest_point = swri_geometry_util::ProjectToLineSegment(
obstacle.polygon.back(),
obstacle.polygon.front(),
point.position());
}
}
if (distance > p.max_distance_m_) {
// The obstacle is too far away from this point to be a concern
continue;
}
// This obstacle is close enough to be a concern. If the bounding
// circles are still not touching, we apply a speed limit based on the
// distance.
if (distance > 0.0) {
DistanceReport report;
report.near = false;
report.collision = false;
report.route_index = route_index;
report.vehicle_point = point.position() + (closest_point - point.position()).normalized() * veh_r;
report.obstacle_point = closest_point;
reports.push_back(report);
const double s = std::max(0.0, (distance - p.min_distance_m_) / (
p.max_distance_m_ - p.min_distance_m_));
double speed = (1.0-s)*p.min_speed_ + s*p.max_speed_;
speeds.speeds.emplace_back();
speeds.speeds.back().id = point.id();
speeds.speeds.back().distance = 0.0;
speeds.speeds.back().speed = speed;
continue;
}
DistanceReport report;
report.near = false;
report.collision = true;
report.route_index = route_index;
report.vehicle_point = point.position();
report.obstacle_point = closest_point;
reports.push_back(report);
speeds.speeds.emplace_back();
speeds.speeds.back().id = point.id();
speeds.speeds.back().distance = -p.stop_buffer_m_ - p.origin_to_front_m_;
speeds.speeds.back().speed = 0.0;
speeds.speeds.emplace_back();
speeds.speeds.back().id = point.id();
speeds.speeds.back().distance = (-p.stop_buffer_m_ - p.origin_to_front_m_) / 2.0;
speeds.speeds.back().speed = 0.0;
speeds.speeds.emplace_back();
speeds.speeds.back().id = point.id();
speeds.speeds.back().distance = 0.0;
speeds.speeds.back().speed = 0.0;
continue;
}
}
}
