void OpenPlannerSimulator::ConvertFromAutowareCloudClusterObstaclesToPlannerH(const PlannerHNS::WayPoint& currState, const PlannerHNS::CAR_BASIC_INFO& car_info, const autoware_msgs::CloudClusterArray& clusters, std::vector<PlannerHNS::DetectedObject>& obstacles_list, int& nOriginalPoints, int& nContourPoints) { PlannerHNS::Mat3 rotationMat(-currState.pos.a); PlannerHNS::Mat3 translationMat(-currState.pos.x, -currState.pos.y); int nPoints = 0; int nOrPoints = 0; for(unsigned int i =0; i < clusters.clusters.size(); i++) { if(clusters.clusters.at(i).id == m_SimParams.id) { //std::cout << "Skip Same ID " << std::endl; continue; } PolygonGenerator polyGen; PlannerHNS::DetectedObject obj; obj.center.pos = GPSPoint(clusters.clusters.at(i).centroid_point.point.x, clusters.clusters.at(i).centroid_point.point.y, clusters.clusters.at(i).centroid_point.point.z,0); //tf::getYaw(clusters.clusters.at(i).bounding_box.pose.orientation)); pcl::PointCloud<pcl::PointXYZ> point_cloud; pcl::fromROSMsg(clusters.clusters.at(i).cloud, point_cloud); obj.contour = polyGen.EstimateClusterPolygon(point_cloud ,obj.center.pos); obj.w = clusters.clusters.at(i).dimensions.y; obj.l = clusters.clusters.at(i).dimensions.x; obj.h = clusters.clusters.at(i).dimensions.z; obj.id = 0; PlannerHNS::GPSPoint relative_point; relative_point = translationMat*obj.center.pos; relative_point = rotationMat*relative_point; double distance_x = fabs(relative_point.x); double distance_y = fabs(relative_point.y); // double size = (obj.w+obj.l)/2.0; // if(size <= 0.25 || size >= 5 || distance_y > 20.0 || distance_x > 20.0) // continue; // if(distance_x <= car_info.length && distance_y <= car_info.width/1.5) // don't detect yourself // continue; nOrPoints += point_cloud.points.size(); nPoints += obj.contour.size(); //std::cout << " Distance_X: " << distance_x << ", " << " Distance_Y: " << distance_y << ", " << " Size: " << size << std::endl; obstacles_list.push_back(obj); } nOriginalPoints = nOrPoints; nContourPoints = nPoints; }
void renderSprites() { glUseProgram(spriteShaderProgramId); entityManagerRef->visitEntitiesWithTypeMask(componentMask, [&](Entity<EntityManagerTypes...> &entity){ auto &aabbComponent = entity.template getComponent<AABBComponent>(); auto &transformComponent = entity.template getComponent<TransformComponent>(); Eigen::Translation<GLfloat, 3> translationMat((transformComponent.x - HALF_SCREEN_WIDTH) / HALF_SCREEN_WIDTH, (transformComponent.y - HALF_SCREEN_HEIGHT) / HALF_SCREEN_HEIGHT, 0); Eigen::DiagonalMatrix<GLfloat, 3> scaleMat(aabbComponent.width / SCREEN_WIDTH, aabbComponent.height / SCREEN_HEIGHT, 1); Eigen::Transform<GLfloat, 3, Eigen::Affine> transformMatrix = translationMat * scaleMat; boundsSprite.render(transformMatrix.matrix()); }); }
PlannerHNS::WayPoint OpenPlannerSimulator::GetRealCenter(const PlannerHNS::WayPoint& currState) { PlannerHNS::WayPoint pose_center = currState; PlannerHNS::Mat3 rotationMat(-currState.pos.a); PlannerHNS::Mat3 translationMat(-currState.pos.x, -currState.pos.y); PlannerHNS::Mat3 rotationMatInv(currState.pos.a); PlannerHNS::Mat3 translationMatInv(currState.pos.x, currState.pos.y); pose_center.pos = translationMat*pose_center.pos; pose_center.pos = rotationMat*pose_center.pos; pose_center.pos.x += m_CarInfo.wheel_base/3.0; pose_center.pos = rotationMatInv*pose_center.pos; pose_center.pos = translationMatInv*pose_center.pos; return pose_center; }
void TrajectoryCosts::CalculateLateralAndLongitudinalCosts(vector<TrajectoryCost>& trajectoryCosts, const vector<vector<vector<WayPoint> > >& rollOuts, const vector<vector<WayPoint> >& totalPaths, const WayPoint& currState, const vector<WayPoint>& contourPoints, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState) { double critical_lateral_distance = carInfo.width/2.0 + params.horizontalSafetyDistancel; double critical_long_front_distance = carInfo.wheel_base/2.0 + carInfo.length/2.0 + params.verticalSafetyDistance; double critical_long_back_distance = carInfo.length/2.0 + params.verticalSafetyDistance - carInfo.wheel_base/2.0; int iCostIndex = 0; PlannerHNS::Mat3 rotationMat(-currState.pos.a); PlannerHNS::Mat3 translationMat(-currState.pos.x, -currState.pos.y); PlannerHNS::Mat3 invRotationMat(currState.pos.a-M_PI_2); PlannerHNS::Mat3 invTranslationMat(currState.pos.x, currState.pos.y); double corner_slide_distance = critical_lateral_distance/2.0; double ratio_to_angle = corner_slide_distance/carInfo.max_steer_angle; double slide_distance = vehicleState.steer * ratio_to_angle; GPSPoint bottom_left(-critical_lateral_distance ,-critical_long_back_distance, currState.pos.z, 0); GPSPoint bottom_right(critical_lateral_distance, -critical_long_back_distance, currState.pos.z, 0); GPSPoint top_right_car(critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0, currState.pos.z, 0); GPSPoint top_left_car(-critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0, currState.pos.z, 0); GPSPoint top_right(critical_lateral_distance - slide_distance, critical_long_front_distance, currState.pos.z, 0); GPSPoint top_left(-critical_lateral_distance - slide_distance , critical_long_front_distance, currState.pos.z, 0); bottom_left = invRotationMat*bottom_left; bottom_left = invTranslationMat*bottom_left; top_right = invRotationMat*top_right; top_right = invTranslationMat*top_right; bottom_right = invRotationMat*bottom_right; bottom_right = invTranslationMat*bottom_right; top_left = invRotationMat*top_left; top_left = invTranslationMat*top_left; top_right_car = invRotationMat*top_right_car; top_right_car = invTranslationMat*top_right_car; top_left_car = invRotationMat*top_left_car; top_left_car = invTranslationMat*top_left_car; // m_SafetyBox.clear(); // m_SafetyBox.push_back(bottom_left); // m_SafetyBox.push_back(bottom_right); // m_SafetyBox.push_back(top_right); // m_SafetyBox.push_back(top_left); m_SafetyBorder.points.clear(); m_SafetyBorder.points.push_back(bottom_left) ; m_SafetyBorder.points.push_back(bottom_right) ; m_SafetyBorder.points.push_back(top_right_car) ; m_SafetyBorder.points.push_back(top_right) ; m_SafetyBorder.points.push_back(top_left) ; m_SafetyBorder.points.push_back(top_left_car) ; for(unsigned int il=0; il < rollOuts.size(); il++) { if(rollOuts.at(il).size() > 0 && rollOuts.at(il).at(0).size()>0) { RelativeInfo car_info; PlanningHelpers::GetRelativeInfo(totalPaths.at(il), currState, car_info); for(unsigned int it=0; it< rollOuts.at(il).size(); it++) { for(unsigned int icon = 0; icon < contourPoints.size(); icon++) { RelativeInfo obj_info; PlanningHelpers::GetRelativeInfo(totalPaths.at(il), contourPoints.at(icon), obj_info); double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths.at(il), car_info, obj_info); if(obj_info.iFront == 0 && longitudinalDist > 0) longitudinalDist = -longitudinalDist; double close_in_percentage = 1; // close_in_percentage = ((longitudinalDist- critical_long_front_distance)/params.rollInMargin)*4.0; // // if(close_in_percentage <= 0 || close_in_percentage > 1) close_in_percentage = 1; double distance_from_center = trajectoryCosts.at(iCostIndex).distance_from_center; if(close_in_percentage < 1) distance_from_center = distance_from_center - distance_from_center * (1.0-close_in_percentage); double lateralDist = fabs(obj_info.perp_distance - distance_from_center); if(longitudinalDist < -carInfo.length || lateralDist > 6) { continue; } longitudinalDist = longitudinalDist - critical_long_front_distance; if(m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true) trajectoryCosts.at(iCostIndex).bBlocked = true; if(lateralDist <= critical_lateral_distance && longitudinalDist >= -carInfo.length/1.5 && longitudinalDist < params.minFollowingDistance) trajectoryCosts.at(iCostIndex).bBlocked = true; trajectoryCosts.at(iCostIndex).lateral_cost += 1.0/lateralDist; trajectoryCosts.at(iCostIndex).longitudinal_cost += 1.0/fabs(longitudinalDist); if(longitudinalDist >= -critical_long_front_distance && longitudinalDist < trajectoryCosts.at(iCostIndex).closest_obj_distance) { trajectoryCosts.at(iCostIndex).closest_obj_distance = longitudinalDist; trajectoryCosts.at(iCostIndex).closest_obj_velocity = contourPoints.at(icon).v; } } iCostIndex++; } } } }