change apollodriver_hcp2. add tf.

src/apollo/code/apollodriver_hcp2/apollodriver_hcp2.pro

@@ -20,13 +20,14 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += \
         main.cpp \
-        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/localization_msgs/localization.pb.cc  \
-        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/localization_msgs/localization_status.pb.cc  \
-        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/localization_msgs/pose.pb.cc  \
-        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/basic_msgs/header.pb.cc  \
-        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/basic_msgs/pnc_point.pb.cc  \ 
-        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/basic_msgs/geometry.pb.cc  \
-        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/basic_msgs/error_code.pb.cc
+        /apollo_workspace/modules/common_msgs/localization_msgs/localization.pb.cc  \
+        /apollo_workspace/modules/common_msgs/localization_msgs/localization_status.pb.cc  \
+        /apollo_workspace/modules/common_msgs/localization_msgs/pose.pb.cc  \
+        /apollo_workspace/modules/common_msgs/basic_msgs/header.pb.cc  \
+        /apollo_workspace/modules/common_msgs/basic_msgs/pnc_point.pb.cc  \ 
+        /apollo_workspace/modules/common_msgs/basic_msgs/geometry.pb.cc  \
+        /apollo_workspace/modules/common_msgs/basic_msgs/error_code.pb.cc  \
+        /apollo_workspace/bazel-bin/external/apollo_src/modules/common_msgs/transform_msgs/transform.pb.cc
 
 # Default rules for deployment.
 qnx: target.path = /tmp/$${TARGET}/bin
@@ -64,6 +65,8 @@ LIBS += /opt/apollo/neo/lib/cyber/croutine/*.so
 LIBS += /opt/apollo/neo/lib/cyber/event/*.so
 LIBS += /opt/apollo/neo/lib/cyber/time/*.so
 
+LIBS += /opt/apollo/neo/lib/modules/transform/*.so
+
 LIBS += -L/opt/apollo/neo/lib/3rd-fastdds-wrap -lfastrtps
 
 LIBS += -L/lib/aarch64-linux-gnu -lgflags -lglog

src/apollo/code/apollodriver_hcp2/main.cpp

@@ -15,10 +15,16 @@
 #include "modules/common/util/message_util.h"
 #include "modules/common/util/util.h"
 
+#include "modules/transform/transform_broadcaster.h"
+
 
 std::shared_ptr<apollo::cyber::Writer<apollo::localization::LocalizationEstimate>>
       localization_writer_;
 
+apollo::transform::TransformBroadcaster * mtf_broadcaster_;
+
+using apollo::transform::TransformStamped;
+
 using apollo::common::Point3D;
 using apollo::common::math::HeadingToQuaternion;
 using apollo::common::Quaternion;
@@ -81,11 +87,28 @@ void PublishLocalization(const double x,const double y,const double z,const doub
                  pose->mutable_linear_acceleration_vrf());
   localization_writer_->Write(localization);
 
+  TransformStamped rawtransform;
+  TransformStamped * transform = &rawtransform;
+  double timestamp = ::apollo::cyber::Clock::NowInSeconds();
+  transform->mutable_header()->set_timestamp_sec(timestamp);
+  transform->mutable_header()->set_frame_id("world");
+  transform->set_child_frame_id("novatel");
+  auto translation = transform->mutable_transform()->mutable_translation();
+  translation->set_x(x);
+  translation->set_y(y);
+  translation->set_z(z);
+  auto rotation = transform->mutable_transform()->mutable_rotation();
+  rotation->set_qx(cur_orientation.x());
+  rotation->set_qy(cur_orientation.y());
+  rotation->set_qz(cur_orientation.z());
+  rotation->set_qw(cur_orientation.w());
+
+  mtf_broadcaster_->SendTransform(rawtransform);
+
 }
 
 void hcp2callback(iv::rawgps xrawgps)
 {
-//    std::cout<<"call callback. "<<std::endl;
     double flon = xrawgps.mfLon;
     double flat = xrawgps.mfLat;
     
@@ -103,11 +126,12 @@ int main(int argc, char *argv[])
     QCoreApplication a(argc, argv);
 
     apollo::cyber::Init("apllodriver_hcp2");
-    std::unique_ptr<apollo::cyber::Node> pilot_node  = apollo::cyber::CreateNode("apllodriver_hcp2");
+    std::shared_ptr<apollo::cyber::Node> pilot_node  = apollo::cyber::CreateNode("apllodriver_hcp2");
 
     localization_writer_ = pilot_node->CreateWriter<apollo::localization::LocalizationEstimate>("/apollo/localization/adcpose");
+    mtf_broadcaster_ = new apollo::transform::TransformBroadcaster(pilot_node);
 
-    std::string strgpsport =  "/dev/ttyTHS1";
+    std::string strgpsport =  "/dev/ttyUSB0";
     hcp2fun funcall = std::bind(&hcp2callback,std::placeholders::_1);
     hcp2 xhcp2(strgpsport,funcall);