Commit 59304e44 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved several bugs to use the correct curvature for the splines.

parent 1a8d4c27
......@@ -36,7 +36,6 @@ class COpendriveLink
double get_resolution(void) const;
double get_scale_factor(void) const;
double find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point);
double find_closest_local_point(TOpendriveLocalPoint &local,TPoint &point);
double get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world);
double get_track_point_local(TOpendriveTrackPoint &track,TOpendriveLocalPoint &local);
void get_trajectory(std::vector<double> &x, std::vector<double> &y,double start_length=0.0, double end_length=-1.0) const;
......
......@@ -31,6 +31,7 @@ class COpendriveRoadSegment
std::vector<COpendriveRoadSegment *> connecting;
std::string name;
unsigned int id;
opendrive_mark_t center_mark;
protected:
COpendriveRoadSegment();
COpendriveRoadSegment(const COpendriveRoadSegment &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoad *road_ref);
......
......@@ -296,12 +296,10 @@ TOpendriveWorldPoint COpendriveLane::get_start_point(void) const
else
{
track_point.t=this->get_offset()+this->get_width()/2.0;
track_point.s=0.0;
for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
{
track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
}
}
if(this->get_id()>0)
world_point.heading=normalize_angle(world_point.heading+3.14159);
......
......@@ -61,7 +61,6 @@ void COpendriveLink::generate(double start_curvature,double end_curvature,double
end.y=node_end.y;
end.heading=node_end.heading;
end.curvature=end_curvature;
// std::cout << start.x << "," << start.y << "," << start.heading << "," << start_curvature << "," << end.x << "," << end.y << "," << end.heading << "," << end_curvature << std::endl;
this->spline=new CG2Spline(start,end);
this->spline->generate(this->resolution,length);
}
......@@ -99,12 +98,14 @@ double COpendriveLink::get_scale_factor(void) const
double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point)
{
double length;
}
double COpendriveLink::find_closest_local_point(TOpendriveLocalPoint &local,TPoint &point)
{
if(this->spline!=NULL)
length=this->spline->find_closest_point(world.x,world.y,point);
else
length=std::numeric_limits<double>::max();
return length;
}
double COpendriveLink::get_track_point_world(TOpendriveTrackPoint &track,TOpendriveWorldPoint &world)
......
......@@ -61,7 +61,10 @@ void COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
node_start=node->get_start_pose();
for(unsigned int i=0;i<this->lanes.size();i++)
{
lane_end=this->lanes[i]->get_start_point();
if(this->lanes[i]->get_id()<0)
lane_end=this->lanes[i]->get_end_point();
else
lane_end=this->lanes[i]->get_start_point();
if(fabs(lane_end.x-node_start.x)<this->resolution && fabs(lane_end.y-node_start.y)<this->resolution)
{
this->geometries[i]->get_curvature(start_curvature,end_curvature);
......
......@@ -252,7 +252,6 @@ void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
{
opendrive_mark_t center_mark;
center::lane_type center_lane;
if(lane_section.center().lane().present())
......@@ -261,30 +260,30 @@ void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_
if(center_lane.roadMark().size()>1)
throw CException(_HERE_,"Only one road mark supported for lane");
else if(center_lane.roadMark().size()==0)
center_mark=OD_MARK_NONE;
this->center_mark=OD_MARK_NONE;
else if(center_lane.roadMark().size()==1)
{
if(center_lane.roadMark().begin()->type1().present())
{
if(center_lane.roadMark().begin()->type1().get()=="none")
center_mark=OD_MARK_NONE;
this->center_mark=OD_MARK_NONE;
else if(center_lane.roadMark().begin()->type1().get()=="solid")
center_mark=OD_MARK_SOLID;
this->center_mark=OD_MARK_SOLID;
else if(center_lane.roadMark().begin()->type1().get()=="broken")
center_mark=OD_MARK_BROKEN;
this->center_mark=OD_MARK_BROKEN;
else if(center_lane.roadMark().begin()->type1().get()=="solid solid")
center_mark=OD_MARK_SOLID_SOLID;
this->center_mark=OD_MARK_SOLID_SOLID;
else if(center_lane.roadMark().begin()->type1().get()=="solid broken")
center_mark=OD_MARK_SOLID_BROKEN;
this->center_mark=OD_MARK_SOLID_BROKEN;
else if(center_lane.roadMark().begin()->type1().get()=="broken solid")
center_mark=OD_MARK_BROKEN_SOLID;
this->center_mark=OD_MARK_BROKEN_SOLID;
else if(center_lane.roadMark().begin()->type1().get()=="broken broken")
center_mark=OD_MARK_BROKEN_BROKEN;
this->center_mark=OD_MARK_BROKEN_BROKEN;
else
center_mark=OD_MARK_NONE;
this->center_mark=OD_MARK_NONE;
}
else
center_mark=OD_MARK_NONE;
this->center_mark=OD_MARK_NONE;
}
}
for(int i=-this->num_right_lanes;i<0;i++)
......@@ -522,7 +521,22 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int
TOpendriveLocalPoint COpendriveRoadSegment::transform_to_local_point(const TOpendriveTrackPoint &track)
{
TOpendriveTrackPoint point;
TOpendriveLocalPoint local;
if(track.s<0.0)
throw CException(_HERE_,"Invalid track point");
point=track;
if(this->num_right_lanes>0)
local=this->lanes[-1]->transform_to_local_point(point);
else
{
point.s=this->lanes[1]->get_length()-track.s;
point.heading=normalize_angle(track.heading+3.14159);
local=this->lanes[1]->transform_to_local_point(point);
}
return local;
}
TOpendriveWorldPoint COpendriveRoadSegment::transform_to_world_point(const TOpendriveTrackPoint &track)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment