|
@@ -66,7 +66,9 @@ pilot_autoware_bridge::pilot_autoware_bridge() : Node("pilot_autoware_bridge")
|
|
|
|
|
|
pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/perception/obstacle_segmentation/pointcloud", durable_qos);
|
|
pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/perception/obstacle_segmentation/pointcloud", durable_qos);
|
|
|
|
|
|
- detected_object_with_feature_pub_ = create_publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>("/perception/object_recognition/detection/labeled_clusters", durable_qos);
|
|
|
|
|
|
+
|
|
|
|
+ // detected_object_with_feature_pub_ = create_publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>("/perception/object_recognition/detection/labeled_clusters", durable_qos);
|
|
|
|
+ detected_object_with_feature_pub_ = create_publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>("/perception/object_recognition/detection/objects_with_feature", durable_qos);
|
|
pub_odom_ = create_publisher<Odometry>("/localization/kinematic_state", durable_qos);
|
|
pub_odom_ = create_publisher<Odometry>("/localization/kinematic_state", durable_qos);
|
|
pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", durable_qos);
|
|
pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", durable_qos);
|
|
|
|
|
|
@@ -733,7 +735,9 @@ void pilot_autoware_bridge::ConvertObjAndPub(iv::lidar::objectarray * pobjarray)
|
|
|
|
|
|
pose.pose.position.set__x(pobj->position().x());
|
|
pose.pose.position.set__x(pobj->position().x());
|
|
pose.pose.position.set__y(pobj->position().y());
|
|
pose.pose.position.set__y(pobj->position().y());
|
|
- pose.pose.position.set__z(pobj->position().z());
|
|
|
|
|
|
+ pose.pose.position.set__x(pobj->position().y());
|
|
|
|
+ pose.pose.position.set__y(pobj->position().x() *(-1.0));
|
|
|
|
+ pose.pose.position.set__z(pobj->position().z() * 0.0);
|
|
|
|
|
|
double pitch,roll,yaw;
|
|
double pitch,roll,yaw;
|
|
pitch = 0;
|
|
pitch = 0;
|