change pilot_autoware_bridge. add object info, not complete.

src/driver/driver_pilotautoware/main.cpp

@@ -8,10 +8,12 @@
 void * gpagps;
 void * gpachassis;
 void * gpactrlcmd;
+void * gpalidartrack;
 
 simplesmshare * gpsharegps;
 simplesmshare * gpsharechassis;
 simplesmshare * gpsharectrlcmd;
+simplesmshare * gpsharelidartrack;
 
 bool gbthreadrun = true;
 
@@ -42,6 +44,21 @@ void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int
 
 }
 
+
+void ListenLidarTrack(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+
+    std::cout<<" recv lidar track."<<std::endl;
+
+    gpsharelidartrack->write(strdata,nSize);
+
+
+}
+
+
 void threadupdatectrl()
 {
     char * strdata;
@@ -73,10 +90,15 @@ int main(int argc, char *argv[])
 
     gpsharectrlcmd = new simplesmshare("aw_ctrlcmd",10000);
 
+    gpsharelidartrack = new simplesmshare("aw_lidartrack",20000000);
+    gpsharelidartrack->create();
+    gpsharelidartrack->attach();
+
 
     gpagps = iv::modulecomm::RegisterRecv("hcp2_gpsimu",ListenRaw);
     gpachassis = iv::modulecomm::RegisterRecv("chassis",UpdateChassis);
     gpactrlcmd = iv::modulecomm::RegisterSend("ctrlcmd",10000,1);
+    gpalidartrack = iv::modulecomm::RegisterRecv("lidar_track",ListenLidarTrack);
 
     std::thread * pthread = new std::thread(threadupdatectrl);
 

src/ros2/src/pilot_autoware_bridge/CMakeLists.txt

@@ -52,6 +52,8 @@ add_executable(pilot_autoware_bridge
   ./../include/msgtype/gpsimu.pb.cc
   ./../include/msgtype/autowarectrlcmd.pb.cc
   ./../include/msgtype/chassis.pb.cc
+  ./../include/msgtype/object.pb.cc
+  ./../include/msgtype/objectarray.pb.cc
 )
 
 target_link_libraries(pilot_autoware_bridge  ${Protobuf_LIBRARIES}  Geographic )  

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

@@ -62,6 +62,9 @@
 
 #include <string>
 
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 #include "tier4_autoware_utils/ros/msg_covariance.hpp"
 #include "tier4_autoware_utils/ros/update_param.hpp"
@@ -76,6 +79,7 @@
 #include "gpsimu.pb.h"
 #include "autowarectrlcmd.pb.h"
 #include "chassis.pb.h"
+#include "objectarray.pb.h"
 
 //#include "modulecomm.h"
 
@@ -115,83 +119,92 @@ using autoware_auto_control_msgs::msg::AckermannControlCommand;
 class pilot_autoware_bridge : public rclcpp::Node
 {
 public:
-  pilot_autoware_bridge();
-  ~pilot_autoware_bridge() = default;
+    pilot_autoware_bridge();
+    ~pilot_autoware_bridge() = default;
 
 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();
+    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();
+    void callbackTimerTestSim();
 
-  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
 
-  rclcpp::Subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr
+    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_;
-  rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr pub_acc_;
-  rclcpp::Publisher<Imu>::SharedPtr pub_imu_;
-  rclcpp::Publisher<ControlModeReport>::SharedPtr pub_control_mode_report_;
-  rclcpp::Publisher<GearReport>::SharedPtr pub_gear_report_;
-  rclcpp::Publisher<TurnIndicatorsReport>::SharedPtr pub_turn_indicators_report_;
-  rclcpp::Publisher<HazardLightsReport>::SharedPtr pub_hazard_lights_report_;
-  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
-  rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
+    rclcpp::Publisher<Odometry>::SharedPtr pub_odom_;
+    rclcpp::Publisher<SteeringReport>::SharedPtr pub_steer_;
+    rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr pub_acc_;
+    rclcpp::Publisher<Imu>::SharedPtr pub_imu_;
+    rclcpp::Publisher<ControlModeReport>::SharedPtr pub_control_mode_report_;
+    rclcpp::Publisher<GearReport>::SharedPtr pub_gear_report_;
+    rclcpp::Publisher<TurnIndicatorsReport>::SharedPtr pub_turn_indicators_report_;
+    rclcpp::Publisher<HazardLightsReport>::SharedPtr pub_hazard_lights_report_;
+    rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
+    rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
 
-  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+    rclcpp::Publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr detected_object_with_feature_pub_;
+    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
 
-  rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_cmd_;
+    rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_cmd_;
 
 
 
 
-  rclcpp::TimerBase::SharedPtr mptimer;
-  rclcpp::TimerBase::SharedPtr mptimerGPS;
+    rclcpp::TimerBase::SharedPtr mptimer;
+    rclcpp::TimerBase::SharedPtr mptimerGPS;
+    rclcpp::TimerBase::SharedPtr mptimerTestSim;
 
-  void publish_odometry(const Odometry & odometry);
-  void publish_tf(const Odometry & odometry);
+    void publish_odometry(const Odometry & odometry);
+    void publish_tf(const Odometry & odometry);
 
     /**
    * @brief publish velocity
    * @param [in] velocity The velocity report to publish
    */
-  void publish_velocity(const VelocityReport & velocity);
+    void publish_velocity(const VelocityReport & velocity);
 
     /**
    * @brief publish steering
    * @param [in] steer The steering to publish
    */
-  void publish_steering(const SteeringReport & steer);
+    void publish_steering(const SteeringReport & steer);
+
+    void publish_acceleration();
 
-  void publish_acceleration();
 
+    void CalcXY(double flon,double flat,double & fx,double & fy);
 
-  void CalcXY(double flon,double flat,double & fx,double & fy);
+    void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
-  void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ConvertObjAndPub(iv::lidar::objectarray * pobjarray);
 
 
 private:
-  std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
-  std::string origin_frame_id_;     //!< @brief map frame_id
+    std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
+    std::string origin_frame_id_;     //!< @brief map frame_id
+
+    SteeringReport current_steer_;
 
-  SteeringReport current_steer_;
+    AckermannControlCommand::SharedPtr mpcmd_ptr;
 
-  AckermannControlCommand::SharedPtr mpcmd_ptr;
+    bool mbcmdupdate = false;
 
-  bool mbcmdupdate = false;
+    void * mpagpsimu;
+    void * mpactrlcmd;
+    bool mbGPSAttach = false;
+    //  QSharedMemory * mpsharegps;
 
-  void * mpagpsimu;
-  void * mpactrlcmd;
-  bool mbGPSAttach = false;
-//  QSharedMemory * mpsharegps;
+    simplesmshare * mpsharegps;
+    simplesmshare * mpsharecmd;
+    simplesmshare * mpsharechassis;
+    simplesmshare * mpsharelidartrack;
 
-  simplesmshare * mpsharegps;
-  simplesmshare * mpsharecmd;
-  simplesmshare * mpsharechassis;
+    bool mbTestSim = true;
 
 
 };

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

@@ -45,54 +45,62 @@ using namespace std;
 
 pilot_autoware_bridge::pilot_autoware_bridge() : Node("pilot_autoware_bridge")
 {
-  QString str;
-  using std::placeholders::_1;
+    QString str;
+    using std::placeholders::_1;
 
-  simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
-  origin_frame_id_ = declare_parameter("origin_frame_id", "map");
+    simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
+    origin_frame_id_ = declare_parameter("origin_frame_id", "map");
 
-  static constexpr std::size_t queue_size = 1;
-  rclcpp::QoS durable_qos(queue_size);
-  durable_qos.transient_local();
+    static constexpr std::size_t queue_size = 1;
+    rclcpp::QoS durable_qos(queue_size);
+    durable_qos.transient_local();
 
-  twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", durable_qos);
-  // Note: this callback publishes topics above
-  pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
-    "pose", queue_size, std::bind(&pilot_autoware_bridge::callbackPose, this, _1));
+    twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", durable_qos);
+    // Note: this callback publishes topics above
+    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));
 
 
-pub_odom_ = create_publisher<Odometry>("/localization/kinematic_state", durable_qos);
-pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", durable_qos);
+    pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/perception/obstacle_segmentation/pointcloud", durable_qos);
 
-pub_velocity_ = create_publisher<VelocityReport>("/vehicle/status/velocity_status", durable_qos);
+    detected_object_with_feature_pub_ = create_publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>("/perception/object_recognition/detection/labeled_clusters", durable_qos);
+    pub_odom_ = create_publisher<Odometry>("/localization/kinematic_state", durable_qos);
+    pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", durable_qos);
 
-pub_steer_ = create_publisher<SteeringReport>("/vehicle/status/steering_status", durable_qos);
-pub_acc_ = create_publisher<AccelWithCovarianceStamped>("/localization/acceleration", durable_qos);
+    pub_velocity_ = create_publisher<VelocityReport>("/vehicle/status/velocity_status", durable_qos);
 
-sub_cmd_ = create_subscription<AckermannControlCommand>(
-    "/control/command/control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { mpcmd_ptr = msg;mbcmdupdate = true; std::cout<<"new cmd. "<<std::endl;});
+    pub_steer_ = create_publisher<SteeringReport>("/vehicle/status/steering_status", durable_qos);
+    pub_acc_ = create_publisher<AccelWithCovarianceStamped>("/localization/acceleration", durable_qos);
 
+    sub_cmd_ = create_subscription<AckermannControlCommand>(
+                "/control/command/control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { mpcmd_ptr = msg;mbcmdupdate = true; std::cout<<"new cmd. "<<std::endl;});
 
- mpsharecmd =  new simplesmshare("aw_ctrlcmd",1000);
- mpsharecmd->create();
- mpsharecmd->attach();
 
- mpsharechassis = new simplesmshare("aw_chassis",1000);
- mpsharegps = new simplesmshare("aw_gps",10000);
+    mpsharecmd =  new simplesmshare("aw_ctrlcmd",1000);
+    mpsharecmd->create();
+    mpsharecmd->attach();
 
+    mpsharechassis = new simplesmshare("aw_chassis",1000);
+    mpsharegps = new simplesmshare("aw_gps",10000);
+    mpsharelidartrack = new simplesmshare("aw_lidartrack",20000000);
 
-  mptimer= create_wall_timer(100ms,std::bind(&pilot_autoware_bridge::callbackTimer, this));
-  mptimerGPS = create_wall_timer(10ms,std::bind(&pilot_autoware_bridge::callbackTimerGPS, this));
 
-  
+    mptimer= create_wall_timer(100ms,std::bind(&pilot_autoware_bridge::callbackTimer, this));
+    mptimerGPS = create_wall_timer(10ms,std::bind(&pilot_autoware_bridge::callbackTimerGPS, this));
 
-// ModuleFun fungpsimu =std::bind(&pilot_autoware_bridge::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-// mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
 
-// mpactrlcmd = iv::modulecomm::RegisterSend("ctrlcmd",1000,1);
+
+    if(mbTestSim)
+    {
+        mptimerTestSim = create_wall_timer(10ms,std::bind(&pilot_autoware_bridge::callbackTimerTestSim,this));
+    }
+    // ModuleFun fungpsimu =std::bind(&pilot_autoware_bridge::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    // mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
+
+    // mpactrlcmd = iv::modulecomm::RegisterSend("ctrlcmd",1000,1);
 
 
 
@@ -101,67 +109,67 @@ sub_cmd_ = create_subscription<AckermannControlCommand>(
 
 double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
 {
-  double diff_rad = lhs_rad - rhs_rad;
-  if (diff_rad > M_PI) {
-    diff_rad = diff_rad - 2 * M_PI;
-  } else if (diff_rad < -M_PI) {
-    diff_rad = diff_rad + 2 * M_PI;
-  }
-  return diff_rad;
+    double diff_rad = lhs_rad - rhs_rad;
+    if (diff_rad > M_PI) {
+        diff_rad = diff_rad - 2 * M_PI;
+    } else if (diff_rad < -M_PI) {
+        diff_rad = diff_rad + 2 * M_PI;
+    }
+    return diff_rad;
 }
 
 // x: roll, y: pitch, z: yaw
 geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose)
 {
-  geometry_msgs::msg::Vector3 rpy;
-  tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
-  tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
-  return rpy;
+    geometry_msgs::msg::Vector3 rpy;
+    tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
+    tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
+    return rpy;
 }
 
 geometry_msgs::msg::Vector3 getRPY(geometry_msgs::msg::PoseStamped::SharedPtr pose)
 {
-  return getRPY(pose->pose);
+    return getRPY(pose->pose);
 }
 
 geometry_msgs::msg::TwistStamped calcTwist(
-  geometry_msgs::msg::PoseStamped::SharedPtr pose_a,
-  geometry_msgs::msg::PoseStamped::SharedPtr pose_b)
+        geometry_msgs::msg::PoseStamped::SharedPtr pose_a,
+        geometry_msgs::msg::PoseStamped::SharedPtr pose_b)
 {
-  const double dt =
-    (rclcpp::Time(pose_b->header.stamp) - rclcpp::Time(pose_a->header.stamp)).seconds();
+    const double dt =
+            (rclcpp::Time(pose_b->header.stamp) - rclcpp::Time(pose_a->header.stamp)).seconds();
+
+    if (dt == 0) {
+        geometry_msgs::msg::TwistStamped twist;
+        twist.header = pose_b->header;
+        return twist;
+    }
+
+    const auto pose_a_rpy = getRPY(pose_a);
+    const auto pose_b_rpy = getRPY(pose_b);
+
+    geometry_msgs::msg::Vector3 diff_xyz;
+    geometry_msgs::msg::Vector3 diff_rpy;
+
+    diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x;
+    diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y;
+    diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z;
+    diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x);
+    diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y);
+    diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z);
 
-  if (dt == 0) {
     geometry_msgs::msg::TwistStamped twist;
     twist.header = pose_b->header;
+    twist.twist.linear.x =
+            std::sqrt(std::pow(diff_xyz.x, 2.0) + std::pow(diff_xyz.y, 2.0) + std::pow(diff_xyz.z, 2.0)) /
+            dt;
+    twist.twist.linear.y = 0;
+    twist.twist.linear.z = 0;
+    twist.twist.angular.x = diff_rpy.x / dt;
+    twist.twist.angular.y = diff_rpy.y / dt;
+    twist.twist.angular.z = diff_rpy.z / dt;
+
     return twist;
-  }
-
-  const auto pose_a_rpy = getRPY(pose_a);
-  const auto pose_b_rpy = getRPY(pose_b);
-
-  geometry_msgs::msg::Vector3 diff_xyz;
-  geometry_msgs::msg::Vector3 diff_rpy;
-
-  diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x;
-  diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y;
-  diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z;
-  diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x);
-  diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y);
-  diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z);
-
-  geometry_msgs::msg::TwistStamped twist;
-  twist.header = pose_b->header;
-  twist.twist.linear.x =
-    std::sqrt(std::pow(diff_xyz.x, 2.0) + std::pow(diff_xyz.y, 2.0) + std::pow(diff_xyz.z, 2.0)) /
-    dt;
-  twist.twist.linear.y = 0;
-  twist.twist.linear.z = 0;
-  twist.twist.angular.x = diff_rpy.x / dt;
-  twist.twist.angular.y = diff_rpy.y / dt;
-  twist.twist.angular.z = diff_rpy.z / dt;
-
-  return twist;
 }
 
 struct Quaternion {
@@ -220,10 +228,10 @@ Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch
 
 void pilot_autoware_bridge::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr)
 {
-  (void)pose_msg_ptr;
-  // TODO(YamatoAndo) check time stamp diff
-  // TODO(YamatoAndo) check suddenly move
-  // TODO(YamatoAndo) apply low pass filter
+    (void)pose_msg_ptr;
+    // TODO(YamatoAndo) check time stamp diff
+    // TODO(YamatoAndo) check suddenly move
+    // TODO(YamatoAndo) apply low pass filter
 
 
 }
@@ -232,9 +240,9 @@ void pilot_autoware_bridge::callbackPose(geometry_msgs::msg::PoseStamped::Shared
 void pilot_autoware_bridge::callbackTimerGPS()
 {
 
-  
+
     unsigned int nSize;
-    char * strdata = new char[10000];
+    char * strdata = new char[20000000];
 
     int nread = mpsharegps->read(strdata,10000);
 
@@ -245,99 +253,118 @@ void pilot_autoware_bridge::callbackTimerGPS()
     else
     {
 
-    nSize = nread;
-    
-    std::cout<<" nSize: "<<nSize<<std::endl;
-    
-       iv::gps::gpsimu  xgpsimu;
-    if(!xgpsimu.ParseFromArray(strdata,nSize))
-    {
-        std::cout<<" parse gps msg fail."<<std::endl;
-        return;
-    }
-    else
-    {
+        nSize = nread;
 
-    double flat = xgpsimu.lat();
-    double flon = xgpsimu.lon();  
+        std::cout<<" nSize: "<<nSize<<std::endl;
 
-    double pitch,roll,yaw;
-    pitch = 0;
-    roll = 0;
+        iv::gps::gpsimu  xgpsimu;
+        if(!xgpsimu.ParseFromArray(strdata,nSize))
+        {
+            std::cout<<" parse gps msg fail."<<std::endl;
+            return;
+        }
+        else
+        {
 
-    yaw = (90 - xgpsimu.heading()) *M_PI/180.0;
-    Quaternion quat = ToQuaternion(yaw,pitch,roll);
+            double flat = xgpsimu.lat();
+            double flon = xgpsimu.lon();
 
-    nav_msgs::msg::Odometry msg;
+            double pitch,roll,yaw;
+            pitch = 0;
+            roll = 0;
 
-    double fx,fy;
-    CalcXY(flon,flat,fx,fy);
+            yaw = (90 - xgpsimu.heading()) *M_PI/180.0;
+            Quaternion quat = ToQuaternion(yaw,pitch,roll);
 
-    double rel_x = fx;//7600;
-    double rel_y = fy;//32100;
-    double rel_z = 0;
-    double vn = xgpsimu.vn();
-    double ve = xgpsimu.ve();
-   // rel_x+=0.01;
+            nav_msgs::msg::Odometry msg;
 
-    msg.pose.pose.position.x = rel_x;
-    msg.pose.pose.position.y = rel_y;
-    msg.pose.pose.position.z = rel_z;
+            double fx,fy;
+            CalcXY(flon,flat,fx,fy);
 
-    msg.pose.pose.orientation.x = quat.x;
-    msg.pose.pose.orientation.y = quat.y;
-    msg.pose.pose.orientation.z = quat.z;
-    msg.pose.pose.orientation.w = quat.w;
+            double rel_x = fx;//7600;
+            double rel_y = fy;//32100;
+            double rel_z = 0;
+            double vn = xgpsimu.vn();
+            double ve = xgpsimu.ve();
+            // rel_x+=0.01;
 
-    msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
+            msg.pose.pose.position.x = rel_x;
+            msg.pose.pose.position.y = rel_y;
+            msg.pose.pose.position.z = rel_z;
 
-    publish_odometry(msg);
-    publish_tf(msg);
+            msg.pose.pose.orientation.x = quat.x;
+            msg.pose.pose.orientation.y = quat.y;
+            msg.pose.pose.orientation.z = quat.z;
+            msg.pose.pose.orientation.w = quat.w;
 
-      autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
-  velocity.longitudinal_velocity = 0;
-  velocity.lateral_velocity = 0.0F;
-  velocity.heading_rate = 0;
-  publish_velocity(velocity);
+            msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
 
-    current_steer_.steering_tire_angle = static_cast<double>(steear_now);
-  current_steer_.stamp = get_clock()->now();
+            publish_odometry(msg);
+            publish_tf(msg);
 
-  publish_steering(current_steer_);
+            autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
+            velocity.longitudinal_velocity = 0;
+            velocity.lateral_velocity = 0.0F;
+            velocity.heading_rate = 0;
+            publish_velocity(velocity);
 
-  publish_acceleration();
-    
-    
-    std::cout<<" rec gps."<<std::endl;
-  }
+            current_steer_.steering_tire_angle = static_cast<double>(steear_now);
+            current_steer_.stamp = get_clock()->now();
+
+            publish_steering(current_steer_);
 
-  }
+            publish_acceleration();
 
 
-  nread = mpsharechassis->read(strdata,10000);
+            std::cout<<" rec gps."<<std::endl;
+        }
+
+    }
 
-  if(nread < 1)
-  {
 
-  }
-  else
-  {
+    nread = mpsharechassis->read(strdata,10000);
+
+    if(nread < 1)
+    {
+
+    }
+    else
+    {
         iv::chassis xchassis;
-    //    static int ncount = 0;
-    if(!xchassis.ParseFromArray(strdata,nread))
+        //    static int ncount = 0;
+        if(!xchassis.ParseFromArray(strdata,nread))
+        {
+            std::cout<<"Chassis ParseFrom Array Error."<<std::endl;
+        }
+        else
+        {
+            steear_now = (xchassis.angle_feedback() *M_PI/180.0)/15.0;
+            std::cout<<" steer : "<<steear_now<<std::endl;
+        }
+
+    }
+
+    nread = mpsharelidartrack->read(strdata,20000000);
+    if(nread<1)
     {
-        std::cout<<"Chassis ParseFrom Array Error."<<std::endl;
+
     }
     else
     {
-        steear_now = (xchassis.angle_feedback() *M_PI/180.0)/15.0;
-        std::cout<<" steer : "<<steear_now<<std::endl;
+        iv::lidar::objectarray xobjarray;
+        if(!xobjarray.ParseFromArray(strdata,nread))
+        {
+            std::cout<<"Lidar Object Parse From Array Fail."<<std::endl;
+        }
+        else
+        {
+
+            ConvertObjAndPub(&xobjarray);
+        }
     }
 
-  }
+    delete strdata;
 
-  delete strdata;
-  
 
 
 
@@ -346,96 +373,46 @@ void pilot_autoware_bridge::callbackTimerGPS()
 
 void pilot_autoware_bridge::callbackTimer()
 {
-  std::cout<<" testpose1. "<<std::endl;
-
-
-  if(mbcmdupdate == true)
-  {
-  const auto steer = mpcmd_ptr->lateral.steering_tire_angle;
-  const auto vel = mpcmd_ptr->longitudinal.speed;
-  const auto accel = mpcmd_ptr->longitudinal.acceleration;
-  iv::autoware::autowarectrlcmd xcmd;
-  xcmd.set_linear_acceleration(accel);
-  xcmd.set_linear_velocity(vel);
-  xcmd.set_steering_angle(steer);
- // steear_now = steer;
-  int ndatasize = xcmd.ByteSizeLong();
-  std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[ndatasize]);
-  if(xcmd.SerializeToArray(pstr_ptr.get(),ndatasize))
-  {
-    mpsharecmd->write(pstr_ptr.get(),ndatasize);
-  }
-  else
-  {
-    std::cout<<" cmd serialize fail."<<std::endl;
-  }
-  std::cout<<" stear: "<<steer<<" vel: "<<vel<<" accel: "<<accel<<std::endl;
-  mbcmdupdate = false;
-  }
-  return;
-
-   double pitch,roll,yaw;
-    pitch = 0;
-    roll = 0;
-
-    yaw = 0;
-    Quaternion quat = ToQuaternion(yaw,pitch,roll);
-
-    nav_msgs::msg::Odometry msg;
-
-    double flon,flat;
-    flon = 117.0857970;
-    flat = 39.1366668;
-
-    double fx,fy;
-    CalcXY(flon,flat,fx,fy);
-
-    static double rel_x = fx;//7600;
-    static double rel_y = fy;//32100;
-    static double rel_z = 0;
-    static double vn =  0;
-    static double ve =  1;
-   // rel_x+=0.01;
-
-    msg.pose.pose.position.x = rel_x;
-    msg.pose.pose.position.y = rel_y;
-    msg.pose.pose.position.z = rel_z;
-
-    msg.pose.pose.orientation.x = quat.x;
-    msg.pose.pose.orientation.y = quat.y;
-    msg.pose.pose.orientation.z = quat.z;
-    msg.pose.pose.orientation.w = quat.w;
-
-    msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
-
-    publish_odometry(msg);
-    publish_tf(msg);
+    std::cout<<" testpose1. "<<std::endl;
 
-      autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
-  velocity.longitudinal_velocity = 0;
-  velocity.lateral_velocity = 0.0F;
-  velocity.heading_rate = 0;
-  publish_velocity(velocity);
 
-    current_steer_.steering_tire_angle = static_cast<double>(steear_now);
-  current_steer_.stamp = get_clock()->now();
-
-  publish_steering(current_steer_);
-
-  publish_acceleration();
+    if(mbcmdupdate == true)
+    {
+        const auto steer = mpcmd_ptr->lateral.steering_tire_angle;
+        const auto vel = mpcmd_ptr->longitudinal.speed;
+        const auto accel = mpcmd_ptr->longitudinal.acceleration;
+        iv::autoware::autowarectrlcmd xcmd;
+        xcmd.set_linear_acceleration(accel);
+        xcmd.set_linear_velocity(vel);
+        xcmd.set_steering_angle(steer);
+        // steear_now = steer;
+        int ndatasize = xcmd.ByteSizeLong();
+        std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[ndatasize]);
+        if(xcmd.SerializeToArray(pstr_ptr.get(),ndatasize))
+        {
+            mpsharecmd->write(pstr_ptr.get(),ndatasize);
+        }
+        else
+        {
+            std::cout<<" cmd serialize fail."<<std::endl;
+        }
+        std::cout<<" stear: "<<steer<<" vel: "<<vel<<" accel: "<<accel<<std::endl;
+        mbcmdupdate = false;
+    }
+    return;
 
 
 }
 
 void pilot_autoware_bridge::publish_odometry(const Odometry & odometry)
 {
-  static int frame_id = 0;
-  frame_id++;
-  Odometry msg = odometry;
-  msg.header.frame_id = origin_frame_id_;
-  msg.header.stamp = get_clock()->now();
-  msg.child_frame_id = simulated_frame_id_;
-  pub_odom_->publish(msg);
+    static int frame_id = 0;
+    frame_id++;
+    Odometry msg = odometry;
+    msg.header.frame_id = origin_frame_id_;
+    msg.header.stamp = get_clock()->now();
+    msg.child_frame_id = simulated_frame_id_;
+    pub_odom_->publish(msg);
 
 
 }
@@ -443,27 +420,27 @@ void pilot_autoware_bridge::publish_odometry(const Odometry & odometry)
 void pilot_autoware_bridge::publish_tf(const Odometry & odometry)
 {
     static int frame_id = 0;
-  frame_id++;
-  TransformStamped tf;
-  tf.header.stamp = get_clock()->now();
-  tf.header.frame_id = origin_frame_id_;
-  tf.child_frame_id = simulated_frame_id_;
-  tf.transform.translation.x = odometry.pose.pose.position.x;
-  tf.transform.translation.y = odometry.pose.pose.position.y;
-  tf.transform.translation.z = odometry.pose.pose.position.z;
-  tf.transform.rotation = odometry.pose.pose.orientation;
-
-  tf2_msgs::msg::TFMessage tf_msg{};
-  tf_msg.transforms.emplace_back(std::move(tf));
-  pub_tf_->publish(tf_msg);
+    frame_id++;
+    TransformStamped tf;
+    tf.header.stamp = get_clock()->now();
+    tf.header.frame_id = origin_frame_id_;
+    tf.child_frame_id = simulated_frame_id_;
+    tf.transform.translation.x = odometry.pose.pose.position.x;
+    tf.transform.translation.y = odometry.pose.pose.position.y;
+    tf.transform.translation.z = odometry.pose.pose.position.z;
+    tf.transform.rotation = odometry.pose.pose.orientation;
+
+    tf2_msgs::msg::TFMessage tf_msg{};
+    tf_msg.transforms.emplace_back(std::move(tf));
+    pub_tf_->publish(tf_msg);
 }
 
 void pilot_autoware_bridge::publish_velocity(const VelocityReport & velocity)
 {
-  VelocityReport msg = velocity;
-  msg.header.stamp = get_clock()->now();
-  msg.header.frame_id = simulated_frame_id_;
-  pub_velocity_->publish(msg);
+    VelocityReport msg = velocity;
+    msg.header.stamp = get_clock()->now();
+    msg.header.frame_id = simulated_frame_id_;
+    pub_velocity_->publish(msg);
 }
 
 void pilot_autoware_bridge::CalcXY(double flon,double flat,double & fx,double & fy)
@@ -477,7 +454,7 @@ void pilot_autoware_bridge::CalcXY(double flon,double flat,double & fx,double &
     try {
         GeographicLib::UTMUPS::Forward(flat, flon, zone, is_north, utm_x, utm_y);
         GeographicLib::MGRS::Forward(
-            zone, is_north, utm_x, utm_y, flat, 3, mgrs_code);
+                    zone, is_north, utm_x, utm_y, flat, 3, mgrs_code);
     } catch (const GeographicLib::GeographicErr & err) {
         std::cerr << err.what() << std::endl;
         return;
@@ -485,7 +462,7 @@ void pilot_autoware_bridge::CalcXY(double flon,double flat,double & fx,double &
     }
     fx = fmod(utm_x,1e5);
     fy = fmod(utm_y,1e5);
-  //  std::cout<<" fx: "<<fx<<" fy: "<<fy<<std::endl;
+    //  std::cout<<" fx: "<<fx<<" fy: "<<fy<<std::endl;
     return;
 }
 
@@ -495,34 +472,34 @@ void pilot_autoware_bridge::CalcXY(double flon,double flat,double & fx,double &
 void pilot_autoware_bridge::publish_steering(const SteeringReport & steer)
 {
 
-  pub_steer_->publish(steer);
+    pub_steer_->publish(steer);
 }
 
 void pilot_autoware_bridge::publish_acceleration()
 {
-  AccelWithCovarianceStamped msg;
-  msg.header.frame_id = "/base_link";
-  msg.header.stamp = get_clock()->now();
-  msg.accel.accel.linear.x = 0;//vehicle_model_ptr_->getAx();
-
-  using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
-  constexpr auto COV = 0.001;
-  msg.accel.covariance.at(COV_IDX::X_X) = COV;          // linear x
-  msg.accel.covariance.at(COV_IDX::Y_Y) = COV;          // linear y
-  msg.accel.covariance.at(COV_IDX::Z_Z) = COV;          // linear z
-  msg.accel.covariance.at(COV_IDX::ROLL_ROLL) = COV;    // angular x
-  msg.accel.covariance.at(COV_IDX::PITCH_PITCH) = COV;  // angular y
-  msg.accel.covariance.at(COV_IDX::YAW_YAW) = COV;      // angular z
-  pub_acc_->publish(msg);
+    AccelWithCovarianceStamped msg;
+    msg.header.frame_id = "/base_link";
+    msg.header.stamp = get_clock()->now();
+    msg.accel.accel.linear.x = 0;//vehicle_model_ptr_->getAx();
+
+    using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+    constexpr auto COV = 0.001;
+    msg.accel.covariance.at(COV_IDX::X_X) = COV;          // linear x
+    msg.accel.covariance.at(COV_IDX::Y_Y) = COV;          // linear y
+    msg.accel.covariance.at(COV_IDX::Z_Z) = COV;          // linear z
+    msg.accel.covariance.at(COV_IDX::ROLL_ROLL) = COV;    // angular x
+    msg.accel.covariance.at(COV_IDX::PITCH_PITCH) = COV;  // angular y
+    msg.accel.covariance.at(COV_IDX::YAW_YAW) = COV;      // angular z
+    pub_acc_->publish(msg);
 }
 
 void pilot_autoware_bridge::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
-  std::cout<<" recv gps."<<std::endl;
+    std::cout<<" recv gps."<<std::endl;
 
-  (void)index;
-  (void)dt;
-  (void)strmemname;
+    (void)index;
+    (void)dt;
+    (void)strmemname;
     iv::gps::gpsimu  xgpsimu;
     if(!xgpsimu.ParseFromArray(strdata,nSize))
     {
@@ -531,7 +508,7 @@ void pilot_autoware_bridge::UpdateGPSIMU(const char * strdata,const unsigned int
     }
 
     double flat = xgpsimu.lat();
-    double flon = xgpsimu.lon();  
+    double flon = xgpsimu.lon();
 
     double pitch,roll,yaw;
     pitch = 0;
@@ -550,7 +527,7 @@ void pilot_autoware_bridge::UpdateGPSIMU(const char * strdata,const unsigned int
     static double rel_z = 0;
     static double vn = 0;
     static double ve = 1;
-   // rel_x+=0.01;
+    // rel_x+=0.01;
 
     msg.pose.pose.position.x = rel_x;
     msg.pose.pose.position.y = rel_y;
@@ -566,18 +543,18 @@ void pilot_autoware_bridge::UpdateGPSIMU(const char * strdata,const unsigned int
     publish_odometry(msg);
     publish_tf(msg);
 
-      autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
-  velocity.longitudinal_velocity = 0;
-  velocity.lateral_velocity = 0.0F;
-  velocity.heading_rate = 0;
-  publish_velocity(velocity);
+    autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
+    velocity.longitudinal_velocity = 0;
+    velocity.lateral_velocity = 0.0F;
+    velocity.heading_rate = 0;
+    publish_velocity(velocity);
 
     current_steer_.steering_tire_angle = static_cast<double>(0);
-  current_steer_.stamp = get_clock()->now();
+    current_steer_.stamp = get_clock()->now();
 
-  publish_steering(current_steer_);
+    publish_steering(current_steer_);
 
-  publish_acceleration();
+    publish_acceleration();
 }
 
 
@@ -618,17 +595,192 @@ void pilot_autoware_bridge::callbackObject(tier4_perception_msgs::msg::DetectedO
             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;
+        //        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;
+    }
+}
+
+void pilot_autoware_bridge::callbackTimerTestSim()
+{
+    double pitch,roll,yaw;
+    pitch = 0;
+    roll = 0;
+
+    yaw = 0;
+    Quaternion quat = ToQuaternion(yaw,pitch,roll);
+
+    nav_msgs::msg::Odometry msg;
+
+    double flon,flat;
+    flon = 117.0857970;
+    flat = 39.1366668;
+
+    double fx,fy;
+    CalcXY(flon,flat,fx,fy);
+
+    static double rel_x = fx;//7600;
+    static double rel_y = fy;//32100;
+    static double rel_z = 0;
+    static double vn =  0;
+    static double ve =  1;
+    // rel_x+=0.01;
+
+    msg.pose.pose.position.x = rel_x;
+    msg.pose.pose.position.y = rel_y;
+    msg.pose.pose.position.z = rel_z;
+
+    msg.pose.pose.orientation.x = quat.x;
+    msg.pose.pose.orientation.y = quat.y;
+    msg.pose.pose.orientation.z = quat.z;
+    msg.pose.pose.orientation.w = quat.w;
+
+    msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
+
+    publish_odometry(msg);
+    publish_tf(msg);
+
+    autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
+    velocity.longitudinal_velocity = 0;
+    velocity.lateral_velocity = 0.0F;
+    velocity.heading_rate = 0;
+    publish_velocity(velocity);
+
+    current_steer_.steering_tire_angle = static_cast<double>(steear_now);
+    current_steer_.stamp = get_clock()->now();
+
+    publish_steering(current_steer_);
+
+    publish_acceleration();
+
+    geometry_msgs::msg::PoseWithCovariance pose;
+    geometry_msgs::msg::TwistWithCovariance twist;
+
+    pose.pose.position.set__x(10);
+    pose.pose.position.set__y(0);
+    pose.pose.position.set__z(0);
+
+    pose.pose.orientation.set__w(quat.w);
+    pose.pose.orientation.set__x(quat.x);
+    pose.pose.orientation.set__y(quat.y);
+    pose.pose.orientation.set__z(quat.z);
+
+    twist.twist.linear.set__x(0);
+
+
+    tier4_perception_msgs::msg::DetectedObjectsWithFeature output_dynamic_object_msg;
+
+    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__probability(1.0);
+    feature_object.object.classification.push_back(xclass);
+    feature_object.object.kinematics.pose_with_covariance = pose;
+    feature_object.object.kinematics.twist_with_covariance = twist;
+    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;
+
+
+    rclcpp::Time current_time = this->now();
+    output_dynamic_object_msg.feature_objects.push_back(feature_object);
+    output_dynamic_object_msg.header.frame_id = "base_link";
+    output_dynamic_object_msg.header.stamp = current_time;
+    detected_object_with_feature_pub_->publish(output_dynamic_object_msg);
+
+
+    sensor_msgs::msg::PointCloud2 output_pointcloud_msg;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr merged_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::toROSMsg(*merged_pointcloud_ptr, output_pointcloud_msg);
+
+    output_pointcloud_msg.header.frame_id = "base_link";
+    output_pointcloud_msg.header.stamp = current_time;
+    // publish
+    pointcloud_pub_->publish(output_pointcloud_msg);
+
+
+
+}
+
+void pilot_autoware_bridge::ConvertObjAndPub(iv::lidar::objectarray * pobjarray)
+{
+
+    sensor_msgs::msg::PointCloud2 output_pointcloud_msg;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr merged_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
+
+    tier4_perception_msgs::msg::DetectedObjectsWithFeature output_dynamic_object_msg;
+
+    int nobjsize = pobjarray->obj_size();
+
+    int i;
+    for(i=0;i<nobjsize;i++)
+    {
+        iv::lidar::lidarobject * pobj = pobjarray->mutable_obj(i);
+        geometry_msgs::msg::PoseWithCovariance pose;
+        geometry_msgs::msg::TwistWithCovariance twist;
+
+        pose.pose.position.set__x(pobj->position().x());
+        pose.pose.position.set__y(pobj->position().y());
+        pose.pose.position.set__z(pobj->position().z());
+
+        double pitch,roll,yaw;
+        pitch = 0;
+        roll = 0;
+        yaw = pobj->tyaw();
+        Quaternion quat = ToQuaternion(yaw,pitch,roll);
+
+        pose.pose.orientation.set__w(quat.w);
+        pose.pose.orientation.set__x(quat.x);
+        pose.pose.orientation.set__y(quat.y);
+        pose.pose.orientation.set__z(quat.z);
+
+        twist.twist.linear.set__x(0);
+
+
+
+
+        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__probability(1.0);
+        feature_object.object.classification.push_back(xclass);
+        feature_object.object.kinematics.pose_with_covariance = pose;
+        feature_object.object.kinematics.twist_with_covariance = twist;
+        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;
+
+        output_dynamic_object_msg.feature_objects.push_back(feature_object);
+
     }
+
+
+
+
+
+    rclcpp::Time current_time = this->now();
+
+    output_dynamic_object_msg.header.frame_id = "base_link";
+    output_dynamic_object_msg.header.stamp = current_time;
+    detected_object_with_feature_pub_->publish(output_dynamic_object_msg);
+
+
+
+    pcl::toROSMsg(*merged_pointcloud_ptr, output_pointcloud_msg);
+
+    output_pointcloud_msg.header.frame_id = "base_link";
+    output_pointcloud_msg.header.stamp = current_time;
+    // publish
+    pointcloud_pub_->publish(output_pointcloud_msg);
 }