change some code for test autoware object.

src/ros2/src/pilot_autoware_bridge/CMakeLists.txt

@@ -60,7 +60,8 @@ target_link_libraries(pilot_autoware_bridge  ${Protobuf_LIBRARIES}  Geographic )
 ament_target_dependencies(pilot_autoware_bridge rclcpp std_msgs geometry_msgs 
   tf2_geometry_msgs nav_msgs sensor_msgs autoware_auto_control_msgs autoware_auto_geometry_msgs
   autoware_auto_mapping_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs
-  tier4_autoware_utils)
+  tier4_autoware_utils  autoware_auto_perception_msgs
+  tier4_perception_msgs)
 
 
 install(TARGETS

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

@@ -68,6 +68,11 @@
 
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
+
 #include "gpsimu.pb.h"
 #include "autowarectrlcmd.pb.h"
 #include "chassis.pb.h"
@@ -115,11 +120,15 @@ public:
 
 private:
   void callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr);
+  void callbackObject(tier4_perception_msgs::msg::DetectedObjectsWithFeature::SharedPtr pose_msg_ptr);
   void callbackTimer();
   void callbackTimerGPS();
 
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
 
+  rclcpp::Subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr
+    detected_object_with_feature_sub_;
+
     rclcpp::Publisher<VelocityReport>::SharedPtr pub_velocity_;
   rclcpp::Publisher<Odometry>::SharedPtr pub_odom_;
   rclcpp::Publisher<SteeringReport>::SharedPtr pub_steer_;

src/ros2/src/pilot_autoware_bridge/package.xml

@@ -20,7 +20,7 @@
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_external_api_msgs</depend>
   <depend>tier4_vehicle_msgs</depend>
-
+  <depend>tier4_perception_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>sensor_msgs</depend>

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

@@ -60,6 +60,10 @@ 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));
+
+
 pub_odom_ = create_publisher<Odometry>("/localization/kinematic_state", durable_qos);
 pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", durable_qos);
 
@@ -576,3 +580,55 @@ void pilot_autoware_bridge::UpdateGPSIMU(const char * strdata,const unsigned int
   publish_acceleration();
 }
 
+
+void pilot_autoware_bridge::callbackObject(tier4_perception_msgs::msg::DetectedObjectsWithFeature::SharedPtr obj_msg_ptr)
+{
+    (void)obj_msg_ptr;
+    std::cout<<"recv obj. "<<std::endl;
+    std::cout<<"obj cout: "<<obj_msg_ptr->feature_objects.size()<<std::endl;
+    tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
+    int nsize = obj_msg_ptr->feature_objects.size();
+    int i;
+    for(i=0;i<nsize;i++)
+    {
+        feature_object = obj_msg_ptr->feature_objects.at(i);
+
+        std::cout<<" class:  "<<feature_object.object.classification.size()<<std::endl;
+        if(feature_object.object.classification.size()>0)
+        {
+            std::cout<<" label: "<<(int)feature_object.object.classification.at(0).label<<"probability"<<feature_object.object.classification.at(0).probability<<std::endl;
+        }
+
+        geometry_msgs::msg::PoseWithCovariance pose;
+        geometry_msgs::msg::TwistWithCovariance twist;
+
+        pose = feature_object.object.kinematics.pose_with_covariance;
+        twist = feature_object.object.kinematics.twist_with_covariance;
+        std::cout<<" x:"<< pose.pose.position.x<<" y: "<<pose.pose.position.y<<std::endl;;
+        std::cout<<" vel: "<<twist.twist.linear.x<<std::endl;
+
+        autoware_auto_perception_msgs::msg::Shape shape= feature_object.object.shape;
+        std::cout<<"dim: x:"<<shape.dimensions.x<<" y:"<<shape.dimensions.y<<" z:"<<shape.dimensions.z<<std::endl;
+        std::cout<<" shape type: "<<(int)shape.type<<std::endl;
+        int j;
+        int sizepoint = shape.footprint.points.size();
+        std::cout<<" shape point size: "<<sizepoint<<std::endl;
+        for(j=0;j<sizepoint;j++)
+        {
+            std::cout<<" index"<<j<<" : x: "<<shape.footprint.points.at(j).x<<" y: "<<shape.footprint.points.at(j).y<<std::endl;
+        }
+
+//        feature_object.object.classification.push_back(object.classification);
+//        feature_object.object.kinematics.pose_with_covariance = object.initial_state.pose_covariance;
+//        feature_object.object.kinematics.twist_with_covariance =
+//          object.initial_state.twist_covariance;
+//        feature_object.object.kinematics.orientation_availability =
+//          autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
+//        feature_object.object.kinematics.has_twist = false;
+//        tf2::toMsg(
+//          tf_base_link2noised_moved_object,
+//          feature_object.object.kinematics.pose_with_covariance.pose);
+//        feature_object.object.shape = object.shape;
+    }
+}
+