change simple_planning_simulator & pilot_autoware_bridge. and change obstacle_stop_planner.param.yaml use object. stop obstacle is ok. not complete.

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

@@ -742,7 +742,7 @@ void pilot_autoware_bridge::ConvertObjAndPub(iv::lidar::objectarray * pobjarray)
         double pitch,roll,yaw;
         pitch = 0;
         roll = 0;
-        yaw = pobj->tyaw();
+        yaw = pobj->tyaw() - M_PI/2.0;
         Quaternion quat = ToQuaternion(yaw,pitch,roll);
 
         pose.pose.orientation.set__w(quat.w);
@@ -767,7 +767,13 @@ void pilot_autoware_bridge::ConvertObjAndPub(iv::lidar::objectarray * pobjarray)
 
         tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
         autoware_auto_perception_msgs::msg::ObjectClassification xclass;
-        xclass.set__label(autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN);
+        xclass.set__label(autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN);
+        if(pobj->type_name() == "pedestrian")
+            xclass.set__label(autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN);
+        if(pobj->type_name() == "car")
+            xclass.set__label(autoware_auto_perception_msgs::msg::ObjectClassification::CAR);
+        if(pobj->type_name() == "bike")
+            xclass.set__label(autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE);
         xclass.set__probability(1.0);
         feature_object.object.classification.push_back(xclass);
         feature_object.object.kinematics.pose_with_covariance = pose;

src/tool/simple_planning_simulator/mainwindow.cpp

@@ -621,7 +621,7 @@ void MainWindow::AddPedObj(double x,double y, double fhdg)
     pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
     mpscene->addItem(pitem);
 
-    AddObjToVector(x,y,0,fhdg,fpedlen,fpedwidth,fpedheight,pitem);
+    AddObjToVector(x,y,0,fhdg,fpedlen,fpedwidth,fpedheight,pitem,1);
 }
 
 void MainWindow::AddCarObj(double x,double y, double fhdg)
@@ -656,7 +656,7 @@ void MainWindow::AddCarObj(double x,double y, double fhdg)
     pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
     mpscene->addItem(pitem);
 
-    AddObjToVector(x,y,0,fhdg,fcarlen,fcarwidth,fcarheight,pitem);
+    AddObjToVector(x,y,0,fhdg,fcarlen,fcarwidth,fcarheight,pitem,0);
 
 }
 
@@ -698,7 +698,7 @@ void MainWindow::AddCustomObj(double x,double y, double z,double fhdg,double fle
 }
 
 
-void MainWindow::AddObjToVector(double x,double y, double z,double fhdg,double flen,double fwidth,double fheight,QGraphicsPathItem * pitem)
+void MainWindow::AddObjToVector(double x,double y, double z,double fhdg,double flen,double fwidth,double fheight,QGraphicsPathItem * pitem,int ntype)
 {
     iv::simobj xsimobj;
     xsimobj.mfx = x;
@@ -710,6 +710,7 @@ void MainWindow::AddObjToVector(double x,double y, double z,double fhdg,double f
     xsimobj.mfheight = fheight;
     xsimobj.mpobjitem = pitem;
     xsimobj.nshowid = mnshowidindex;
+    xsimobj.mtype = ntype;
     mnshowidindex++;
     mvectorsimobj.push_back(xsimobj);
     mpCBObj->addItem(QString("Obj")+ QString::number(xsimobj.nshowid));

src/tool/simple_planning_simulator/mainwindow.h

@@ -150,7 +150,7 @@ private:
     void AddCarObj(double x,double y, double fhdg);
     void AddCustomObj(double x,double y, double z,double fhdg,double flen,double fwidth,double fheight);
 
-    void AddObjToVector(double x,double y, double z,double fhdg,double flen,double fwidth,double fheight,QGraphicsPathItem * pitem);
+    void AddObjToVector(double x,double y, double z,double fhdg,double flen,double fwidth,double fheight,QGraphicsPathItem * pitem,int ntype = 2);
     void DelObjFromVector(unsigned int index);
 
     void normalhdg(double & fhdg);

src/tool/simple_planning_simulator/simmodel.cpp

@@ -225,6 +225,15 @@ void simmodel::threadstate()
                 xlidarobj.set_acceleration_linear_y(0);
                 xlidarobj.set_angle(hdg_obj);
                 xlidarobj.set_tyaw(hdg_obj);
+                xlidarobj.set_type_name("unknown");
+                if(xsimobj.mtype == 0)
+                {
+                    xlidarobj.set_type_name("car");
+                }
+                if(xsimobj.mtype == 1)
+                {
+                    xlidarobj.set_type_name("pedestrian");
+                }
                 iv::lidar::PointXYZ xpos;
                 xpos.set_x(x_obj);
                 xpos.set_y(y_obj);
@@ -272,9 +281,10 @@ void simmodel::threadstate()
 
             iv::lidar::Dimension ld;
             iv::lidar::Dimension * pld;
-            ld.set_x(0.1);
-            ld.set_y(0.1);
+            ld.set_x(0.6);
+            ld.set_y(0.6);
             ld.set_z(0.1);
+            xlidarobj.set_type_name("pedestrian");
             //        lidarobj.dimensions.x = ((length < 0) ? -1 * length : length);
             //        lidarobj.dimensions.y = ((width < 0) ? -1 * width : width);
             //        lidarobj.dimensions.z = ((height < 0) ? -1 * height : height);

src/tool/simple_planning_simulator/simmodel.h

@@ -27,6 +27,7 @@ struct simobj
     double mfheight;
     QGraphicsPathItem * mpobjitem;
     int nshowid;
+    int mtype; //0 car 1 ped 2 unknown.
 };
 
 }