change pilot_autoware_bridge. for object. not complete.

src/common/common/xodr/OpenDrive/OpenDriveXmlParser.cpp

@@ -1,6 +1,7 @@
 #include "OpenDriveXmlParser.h"
 #include <iostream>
 #include <algorithm>
+#include <math.h>
 //#include "windows.h"
 
 using std::cout;
@@ -302,6 +303,26 @@ bool OpenDriveXmlParser::ReadRoad(TiXmlElement *node)
         subNode=subNode->NextSiblingElement("userData");
     }
 
+    bool bCheckLen = true;
+    if(bCheckLen)
+    {
+        double flen = 0;
+        unsigned int ngeobcount = static_cast<unsigned int>(road->GetGeometryBlockCount());
+        unsigned int i;
+        for(i=0;i<ngeobcount;i++)
+        {
+            GeometryBlock * pgeob = road->GetGeometryBlock(i);
+            double flen_geob = pgeob->GetBlockLength();
+            flen = flen + flen_geob;
+        }
+
+        if(fabs(flen - road->GetRoadLength())>0.01)
+        {
+            std::cout<<" road: "<<road->GetRoadId()<<" road len : "<<road->GetRoadLength()<<" geo len: "<<flen<<" so fix it. "<<std::endl;
+            road->SetRoadLength(flen);
+        }
+    }
+
 	return true;
 }
 //--------------

src/ros2/src/pilot_autoware_bridge/include/pilot_autoware_bridge/pilot_autoware_bridge_core.hpp

@@ -204,7 +204,7 @@ private:
     simplesmshare * mpsharechassis;
     simplesmshare * mpsharelidartrack;
 
-    bool mbTestSim = true;
+    bool mbTestSim = false;
 
 
 };

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

@@ -60,8 +60,8 @@ pilot_autoware_bridge::pilot_autoware_bridge() : Node("pilot_autoware_bridge")
     pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
                 "pose", queue_size, std::bind(&pilot_autoware_bridge::callbackPose, this, _1));
 
-    // detected_object_with_feature_sub_ = create_subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>(
-    //             "/perception/object_recognition/detection/labeled_clusters", queue_size, std::bind(&pilot_autoware_bridge::callbackObject, this, _1));
+    detected_object_with_feature_sub_ = create_subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>(
+                "/perception/object_recognition/detection/labeled_clusters", queue_size, std::bind(&pilot_autoware_bridge::callbackObject, this, _1));
 
 
     pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/perception/obstacle_segmentation/pointcloud", durable_qos);
@@ -688,6 +688,9 @@ void pilot_autoware_bridge::callbackTimerTestSim()
     feature_object.object.kinematics.orientation_availability =
       autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
     feature_object.object.kinematics.has_twist = false;
+
+
+
  //   feature_object.object.shape = object.shape;
 
 
@@ -770,6 +773,18 @@ void pilot_autoware_bridge::ConvertObjAndPub(iv::lidar::objectarray * pobjarray)
         feature_object.object.kinematics.has_twist = false;
      //   feature_object.object.shape = object.shape;
 
+        // shape
+
+        autoware_auto_perception_msgs::msg::Shape shape;
+        shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX;
+        shape.dimensions.x = pobj->dimensions().x();
+        shape.dimensions.y = pobj->dimensions().y();
+        shape.dimensions.z = pobj->dimensions().z();
+
+        feature_object.object.set__shape(shape);
+
+        std::cout<<" set shape. "<<std::endl;
+
         output_dynamic_object_msg.feature_objects.push_back(feature_object);
 
     }
@@ -791,6 +806,6 @@ void pilot_autoware_bridge::ConvertObjAndPub(iv::lidar::objectarray * pobjarray)
     output_pointcloud_msg.header.frame_id = "base_link";
     output_pointcloud_msg.header.stamp = current_time;
     // publish
-    pointcloud_pub_->publish(output_pointcloud_msg);
+  //  pointcloud_pub_->publish(output_pointcloud_msg);
 }
 

src/tool/simple_planning_simulator/simmodel.cpp

@@ -267,6 +267,8 @@ void simmodel::threadstate()
             plx = xlidarobj.mutable_centroid();
             plx->CopyFrom(xpos);
 
+            xlidarobj.mutable_position()->CopyFrom(xpos);
+
             iv::lidar::Dimension ld;
             iv::lidar::Dimension * pld;
             ld.set_x(0.1);