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