Skip to content
Snippets Groups Projects
Commit e91e16ab authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Get the length at which the desired pose is located on the lane.

parent d81d4839
No related branches found
No related tags found
1 merge request!2Solved a bug when creating a new geometry object: the sale factor is...
...@@ -395,7 +395,7 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t ...@@ -395,7 +395,7 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t
TOpendriveWorldPose start_pose; TOpendriveWorldPose start_pose;
COpendriveRoadNode *new_node; COpendriveRoadNode *new_node;
std::stringstream error; std::stringstream error;
double distance; double distance,length;
if(start==NULL) if(start==NULL)
return; return;
...@@ -406,7 +406,7 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t ...@@ -406,7 +406,7 @@ void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t
throw CException(_HERE_,error.str()); throw CException(_HERE_,error.str());
} }
// get the actual start position on the lane // get the actual start position on the lane
this->get_closest_pose(*start,start_pose,4.71238898); length=this->get_closest_pose(*start,start_pose,4.71238898);
// eliminate all the node before the start one // eliminate all the node before the start one
for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++) for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
{ {
...@@ -444,7 +444,7 @@ void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t & ...@@ -444,7 +444,7 @@ void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &
TOpendriveWorldPose end_pose; TOpendriveWorldPose end_pose;
COpendriveRoadNode *new_node; COpendriveRoadNode *new_node;
std::stringstream error; std::stringstream error;
double distance; double distance,length;
if(end==NULL) if(end==NULL)
return; return;
...@@ -454,7 +454,7 @@ void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t & ...@@ -454,7 +454,7 @@ void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &
error << "No node close to (x=" << end->x << ",y=" << end->y << ",heading=" << end->heading << ") in lane " << this->id << " of segment " << this->segment->get_name(); error << "No node close to (x=" << end->x << ",y=" << end->y << ",heading=" << end->heading << ") in lane " << this->id << " of segment " << this->segment->get_name();
throw CException(_HERE_,error.str()); throw CException(_HERE_,error.str());
} }
this->get_closest_pose(*end,end_pose,4.71238898); length=this->get_closest_pose(*end,end_pose,4.71238898);
// eliminate all the node before the start one // eliminate all the node before the start one
for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++) for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
{ {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment