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