change map_lanetoxodr. add junction crosspath.

src/common/common/xodr/OpenDrive/Junction.cpp

@@ -74,6 +74,14 @@ unsigned int Junction::AddJunctionController ( string id, string type)
 	mLastAddedJunctionController = mJunctionControllerVector.size()-1;
 	return mLastAddedJunctionController;
 }
+
+
+unsigned int Junction::AddJunctionCrossPath(string id,string crossingRoad, string roadAtStart,string roadAtEnd,JunctionCrossPathStartLaneLink startLaneLink,JunctionCrossPathEndLaneLink endLaneLink)
+{
+    mJunctionCrossPathVector.push_back(JunctionCrossPath(id,crossingRoad,roadAtStart,roadAtEnd,startLaneLink,endLaneLink));
+    mLastAddedJunctionCrossPath = mJunctionCrossPathVector.size() -1;
+    return mLastAddedJunctionCrossPath;
+}
 //--------------
 
 /**
@@ -112,6 +120,16 @@ unsigned int Junction::CloneJunctionController(unsigned int index)
 	return mLastAddedJunctionController;
 }
 
+unsigned int Junction::CloneJunctionPath(unsigned int index)
+{
+    if(index<mJunctionCrossPathVector.size()-1)
+        mJunctionCrossPathVector.insert(mJunctionCrossPathVector.begin()+index+1, mJunctionCrossPathVector[index]);
+    else if(index==mJunctionCrossPathVector.size()-1)
+        mJunctionCrossPathVector.push_back(mJunctionCrossPathVector[index]);
+    mLastAddedJunctionCrossPath=index+1;
+    return mLastAddedJunctionCrossPath;
+}
+
 /**
  * Methods used to delete child records from the respective vectors
  */
@@ -128,6 +146,11 @@ void Junction::DeleteJunctionController(unsigned int index)
 	mJunctionControllerVector.erase(mJunctionControllerVector.begin()+index);
 }
 
+void Junction::DeleteJunctionCrossPath(unsigned int index)
+{
+    mJunctionCrossPathVector.erase(mJunctionCrossPathVector.begin()+index);
+}
+
 /**
 * Return the name of the junction
 */
@@ -277,6 +300,34 @@ JunctionController* Junction::GetLastAddedJunctionController()
 	else
 		return NULL;
 }
+
+
+std::vector<JunctionCrossPath>* Junction::GetJunctionCrossPathVector()
+{	return &mJunctionCrossPathVector;	}
+
+unsigned int  Junction::GetJunctionCrossPathCount()
+{	return mJunctionCrossPathVector.size();	}
+
+JunctionCrossPath* Junction::GetJunctionCrossPath(unsigned int i)
+{	return &mJunctionCrossPathVector.at(i);	}
+
+JunctionCrossPath* Junction::GetLastJunctionCrossPath()
+{
+    if(mJunctionCrossPathVector.size()>0)
+        return &mJunctionCrossPathVector.at(mJunctionCrossPathVector.size()-1);
+    else
+        return NULL;
+}
+
+JunctionCrossPath* Junction::GetLastAddedJunctionCrossPath()
+{
+    if(mLastAddedJunctionCrossPath<mJunctionCrossPathVector.size())
+        return &mJunctionCrossPathVector.at(mLastAddedJunctionCrossPath);
+    else
+        return NULL;
+}
+
+
 //--------------
 
 
@@ -438,6 +489,7 @@ JunctionLaneLink* JunctionConnection::GetLastAddedJunctionLaneLink()
 
 
 
+
 /**
 * Junction lane link class. Holds all the information for a lane link record
 *
@@ -562,4 +614,160 @@ string JunctionController::GetId()
 * Get the type of control
 */
 string JunctionController::GetType()
-{	return mType;	}
+{	return mType;	}
+
+
+
+JunctionCrossPathStartLaneLink::JunctionCrossPathStartLaneLink(double s,int  nFrom,int nTo)
+{
+    mS = s;
+    mFrom = nFrom;
+    mTo  = nTo;
+}
+
+JunctionCrossPathStartLaneLink::JunctionCrossPathStartLaneLink()
+{
+    JunctionCrossPathStartLaneLink(0.0,0,0);
+}
+
+double JunctionCrossPathStartLaneLink::GetS()
+{
+    return mS;
+}
+
+int JunctionCrossPathStartLaneLink::GetFrom()
+{
+    return mFrom;
+}
+
+int JunctionCrossPathStartLaneLink::GetTo()
+{
+    return mTo;
+}
+
+void JunctionCrossPathStartLaneLink::SetS(double s)
+{
+    mS = s;
+}
+
+void JunctionCrossPathStartLaneLink::SetFrom(int nFrom)
+{
+    mFrom = nFrom;
+}
+
+void JunctionCrossPathStartLaneLink::SetTo(int nTo)
+{
+    mTo = nTo;
+}
+
+JunctionCrossPathEndLaneLink::JunctionCrossPathEndLaneLink(double s,int  nFrom,int nTo)
+{
+    mS = s;
+    mFrom = nFrom;
+    mTo = nTo;
+}
+
+JunctionCrossPathEndLaneLink::JunctionCrossPathEndLaneLink()
+{
+    JunctionCrossPathEndLaneLink(0.0,0,0);
+}
+
+double JunctionCrossPathEndLaneLink::GetS()
+{
+    return mS;
+}
+
+int JunctionCrossPathEndLaneLink::GetFrom()
+{
+    return mFrom;
+}
+
+int JunctionCrossPathEndLaneLink::GetTo()
+{
+    return mTo;
+}
+
+void JunctionCrossPathEndLaneLink::SetS(double s)
+{
+    mS = s;
+}
+
+void JunctionCrossPathEndLaneLink::SetFrom(int nFrom)
+{
+    mFrom = nFrom;
+}
+
+void JunctionCrossPathEndLaneLink::SetTo(int nTo)
+{
+    mTo = nTo;
+}
+
+JunctionCrossPath::JunctionCrossPath(string id,string crossingRoad, string roadAtStart,string roadAtEnd,JunctionCrossPathStartLaneLink startLaneLink,JunctionCrossPathEndLaneLink endLaneLink)
+{
+    mId = id;
+    mcrossingRoad = crossingRoad;
+    mroadAtStart = roadAtStart;
+    mroadAtEnd = roadAtEnd;
+    mstartLaneLink = startLaneLink;
+    mendLaneLink = endLaneLink;
+}
+
+string JunctionCrossPath::GetId()
+{
+    return mId;
+}
+
+string JunctionCrossPath::GetcrossingRoad()
+{
+    return mcrossingRoad;
+}
+
+string JunctionCrossPath::GetroadAtStart()
+{
+    return mroadAtStart;
+}
+
+string JunctionCrossPath::GetroadAtEnd()
+{
+    return mroadAtEnd;
+}
+
+JunctionCrossPathStartLaneLink JunctionCrossPath::GetstartLaneLink()
+{
+    return mstartLaneLink;
+}
+
+JunctionCrossPathEndLaneLink JunctionCrossPath::GetendLaneLink()
+{
+    return mendLaneLink;
+}
+
+void JunctionCrossPath::SetId(string id)
+{
+    mId = id;
+}
+
+void JunctionCrossPath::SetcrossingRoad(string crossingRoad)
+{
+    mcrossingRoad =  crossingRoad;
+}
+
+void JunctionCrossPath::SetroadAtStart(string roadAtStart)
+{
+    mroadAtStart = roadAtStart;
+}
+
+void JunctionCrossPath::SetroadAtEnd(string roadAtEnd)
+{
+    mroadAtEnd = roadAtEnd;
+}
+
+void JunctionCrossPath::SetJunctionCrossPathStartLaneLink(JunctionCrossPathStartLaneLink xStart)
+{
+    mstartLaneLink = xStart;
+}
+
+void JunctionCrossPath::SetJunctionCrossPathEndLaneLink(JunctionCrossPathEndLaneLink xEnd)
+{
+    mendLaneLink = xEnd;
+}

src/common/common/xodr/OpenDrive/Junction.h

@@ -11,6 +11,9 @@ class JunctionConnection;
 class JunctionLaneLink;
 class JunctionController;
 class JunctionPriorityRoad;
+class JunctionCrossPath;
+class JunctionCrossPathStartLaneLink;
+class JunctionCrossPathEndLaneLink;
 
 /**
  * Junction class. Holds all the junction information
@@ -34,6 +37,7 @@ private:
 	std::vector<JunctionConnection> mJunctionConnectionVector;
 	std::vector<JunctionPriorityRoad> mJunctionPriorityVector;
 	std::vector<JunctionController> mJunctionControllerVector;
+    std::vector<JunctionCrossPath> mJunctionCrossPathVector;
 
 public:
 
@@ -43,6 +47,7 @@ public:
 	unsigned int mLastAddedJunctionConnection;
 	unsigned int mLastAddedJunctionPriority;
 	unsigned int mLastAddedJunctionController;
+    unsigned int mLastAddedJunctionCrossPath;
 
 	/**
 	 * Constructor. Sets basic junction parameters
@@ -83,6 +88,14 @@ public:
 	 */
 	unsigned int AddJunctionController(string id, string type);
 
+    /**
+     *
+     *
+     *
+     *
+     */
+    unsigned int AddJunctionCrossPath(string id,string crossingRoad, string roadAtStart,string roadAtEnd,JunctionCrossPathStartLaneLink startLaneLink,JunctionCrossPathEndLaneLink endLaneLink);
+
 	/**
 	 * Clone the connection record
 	 * @param index Index of the record to clone
@@ -90,11 +103,13 @@ public:
 	unsigned int CloneJunctionConnection(unsigned int index);
 	unsigned int CloneJunctionPriority(unsigned int index);
 	unsigned int CloneJunctionController(unsigned int index);
+    unsigned int CloneJunctionPath(unsigned int index);
 
 
 	void DeleteJunctionConnection(unsigned int index);
 	void DeleteJunctionPriority(unsigned int index);
 	void DeleteJunctionController(unsigned int index);
+    void DeleteJunctionCrossPath(unsigned int index);
 
 	/**
 	 * Return the name of the junction
@@ -200,6 +215,13 @@ public:
 	 */
     JunctionController* GetLastAddedJunctionController();
 
+
+    std::vector<JunctionCrossPath>* GetJunctionCrossPathVector();
+    unsigned int GetJunctionCrossPathCount();
+    JunctionCrossPath * GetJunctionCrossPath(unsigned int i);
+    JunctionCrossPath * GetLastJunctionCrossPath();
+    JunctionCrossPath * GetLastAddedJunctionCrossPath();
+
 };
 
 
@@ -467,4 +489,94 @@ public:
 
 };
 
+
+class JunctionCrossPathStartLaneLink
+{
+private:
+    /**
+     * Record parameters
+     */
+    double mS;
+    int mFrom;
+    int mTo;
+
+public:
+    JunctionCrossPathStartLaneLink(double s,int nFrom,int nTo);
+    JunctionCrossPathStartLaneLink();
+
+    double GetS();
+    int GetFrom();
+    int GetTo();
+
+    void SetS(double s);
+    void SetFrom(int nFrom);
+    void SetTo(int nTo);
+
+};
+
+class JunctionCrossPathEndLaneLink
+{
+private:
+    /**
+     * Record parameters
+     */
+    double mS;
+    int mFrom;
+    int mTo;
+
+public:
+    JunctionCrossPathEndLaneLink(double s,int nFrom,int nTo);
+    JunctionCrossPathEndLaneLink();
+
+    double GetS();
+    int GetFrom();
+    int GetTo();
+
+    void SetS(double s);
+    void SetFrom(int nFrom);
+    void SetTo(int nTo);
+};
+
+
+/**
+ * @brief The JunctionCrossPath class
+ * Define Cross Path from 1.8 standard, used for define crosswalk
+ *
+ */
+class JunctionCrossPath
+{
+private:
+    /**
+     * Connection parameters
+     */
+    string mId;
+    string mcrossingRoad;
+    string mroadAtStart;
+    string mroadAtEnd;	//Possible values: start / end
+    JunctionCrossPathStartLaneLink mstartLaneLink;
+    JunctionCrossPathEndLaneLink mendLaneLink;
+
+public:
+    JunctionCrossPath(string id,string crossingRoad, string roadAtStart,string roadAtEnd,JunctionCrossPathStartLaneLink startLaneLink,JunctionCrossPathEndLaneLink endLaneLink);
+
+    string GetId();
+    string GetcrossingRoad();
+    string GetroadAtStart();
+    string GetroadAtEnd();
+    JunctionCrossPathStartLaneLink GetstartLaneLink();
+    JunctionCrossPathEndLaneLink GetendLaneLink();
+
+    void SetId(string id);
+    void SetcrossingRoad(string crossingRoad);
+    void SetroadAtStart(string roadAtStart);
+    void SetroadAtEnd(string roadAtEnd);
+    void SetJunctionCrossPathStartLaneLink(JunctionCrossPathStartLaneLink xStart);
+    void SetJunctionCrossPathEndLaneLink(JunctionCrossPathEndLaneLink xEnd);
+};
+
+
+
+
+
+
 #endif

src/ros2/src/pilot_autoware_bridge/src/pilot_autoware_bridge_core.cpp

@@ -746,7 +746,17 @@ void pilot_autoware_bridge::ConvertObjAndPub(iv::lidar::objectarray * pobjarray)
         twist.twist.linear.set__x(0);
 
 
-
+        int ncloudsize = pobj->cloud_size();
+        int j;
+        for(j=0;j<ncloudsize;j++)
+        {
+            pcl::PointXYZ point;
+            point.x = pobj->cloud(j).x();
+            point.y = pobj->cloud(j).y();
+            point.z = pobj->cloud(j).z();
+            merged_pointcloud_ptr->points.push_back(point);
+            ++merged_pointcloud_ptr->width;
+        }
 
         tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
         autoware_auto_perception_msgs::msg::ObjectClassification xclass;