|
@@ -2,6 +2,8 @@
|
|
#include "cyber/cyber.h"
|
|
#include "cyber/cyber.h"
|
|
#include "cyber/time/rate.h"
|
|
#include "cyber/time/rate.h"
|
|
#include "cyber/time/time.h"
|
|
#include "cyber/time/time.h"
|
|
|
|
+ #include "modules/common/math/math_utils.h"
|
|
|
|
+#include "modules/common/math/quaternion.h"
|
|
|
|
|
|
#include "modules/common/adapters/adapter_gflags.h"
|
|
#include "modules/common/adapters/adapter_gflags.h"
|
|
|
|
|
|
@@ -9,12 +11,35 @@
|
|
#include "modules/common_msgs/sensor_msgs/ins.pb.h"
|
|
#include "modules/common_msgs/sensor_msgs/ins.pb.h"
|
|
|
|
|
|
#include "modules/common_msgs/localization_msgs/imu.pb.h"
|
|
#include "modules/common_msgs/localization_msgs/imu.pb.h"
|
|
|
|
+
|
|
|
|
+#include "modules/common_msgs/localization_msgs/localization.pb.h"
|
|
|
|
+
|
|
|
|
+#include <GeographicLib/MGRS.hpp>
|
|
|
|
+#include <GeographicLib/UTMUPS.hpp>
|
|
|
|
+
|
|
|
|
+#include "modules/common/math/math_utils.h"
|
|
|
|
+#include "modules/common/math/quaternion.h"
|
|
|
|
+
|
|
|
|
+#include "modules/common/util/message_util.h"
|
|
|
|
+#include "modules/common/util/util.h"
|
|
|
|
|
|
//car数据定义的引用,可以看出其定义来源于一个proto
|
|
//car数据定义的引用,可以看出其定义来源于一个proto
|
|
// using apollo::communication::proto::Car;
|
|
// using apollo::communication::proto::Car;
|
|
|
|
|
|
using ::apollo::localization::Gps;
|
|
using ::apollo::localization::Gps;
|
|
|
|
|
|
|
|
+void testutm()
|
|
|
|
+{
|
|
|
|
+ double x,y;
|
|
|
|
+ int zone{};
|
|
|
|
+ bool is_north{};
|
|
|
|
+
|
|
|
|
+ double flon = 117.0874969;
|
|
|
|
+ double flat = 39.1364644;
|
|
|
|
+ GeographicLib::UTMUPS::Forward(flat, flon, zone, is_north, x,y);
|
|
|
|
+
|
|
|
|
+ std::cout<<" x: "<<x<<" y:"<<y<<std::endl;
|
|
|
|
+}
|
|
|
|
|
|
|
|
|
|
#include <iostream>
|
|
#include <iostream>
|
|
@@ -41,6 +66,9 @@ const char *WGS84_TEXT = "+proj=latlong +ellps=WGS84";
|
|
std::shared_ptr<apollo::cyber::Writer<apollo::localization::Gps>> gps_writer_ = nullptr;
|
|
std::shared_ptr<apollo::cyber::Writer<apollo::localization::Gps>> gps_writer_ = nullptr;
|
|
std::shared_ptr<apollo::cyber::Writer<apollo::drivers::gnss::InsStat>> insstat_writer_ = nullptr;
|
|
std::shared_ptr<apollo::cyber::Writer<apollo::drivers::gnss::InsStat>> insstat_writer_ = nullptr;
|
|
std::shared_ptr<apollo::cyber::Writer<apollo::localization::CorrectedImu>> corrimu_writer_ = nullptr;
|
|
std::shared_ptr<apollo::cyber::Writer<apollo::localization::CorrectedImu>> corrimu_writer_ = nullptr;
|
|
|
|
+
|
|
|
|
+ std::shared_ptr<apollo::cyber::Writer<apollo::localization::LocalizationEstimate>>
|
|
|
|
+ localization_writer_;
|
|
|
|
|
|
//const double DEG_TO_RAD = M_PI / 180.0;
|
|
//const double DEG_TO_RAD = M_PI / 180.0;
|
|
constexpr double DEG_TO_RAD_LOCAL = M_PI / 180.0;
|
|
constexpr double DEG_TO_RAD_LOCAL = M_PI / 180.0;
|
|
@@ -137,6 +165,72 @@ static const int32_t LEAP_SECONDS[][2] = {
|
|
|
|
|
|
const int32_t GPS_AND_SYSTEM_DIFF_SECONDS = 315964800;
|
|
const int32_t GPS_AND_SYSTEM_DIFF_SECONDS = 315964800;
|
|
|
|
|
|
|
|
+std::string stradcpose = "/apollo/localization/adcpose";
|
|
|
|
+
|
|
|
|
+using apollo::common::Point3D;
|
|
|
|
+using apollo::common::math::HeadingToQuaternion;
|
|
|
|
+using apollo::common::Quaternion;
|
|
|
|
+
|
|
|
|
+using apollo::common::math::InverseQuaternionRotate;
|
|
|
|
+using apollo::common::util::FillHeader;
|
|
|
|
+
|
|
|
|
+void TransformToVRF(const Point3D &point_mrf, const Quaternion &orientation,
|
|
|
|
+ Point3D *point_vrf) {
|
|
|
|
+ Eigen::Vector3d v_mrf(point_mrf.x(), point_mrf.y(), point_mrf.z());
|
|
|
|
+ auto v_vrf = InverseQuaternionRotate(orientation, v_mrf);
|
|
|
|
+ point_vrf->set_x(v_vrf.x());
|
|
|
|
+ point_vrf->set_y(v_vrf.y());
|
|
|
|
+ point_vrf->set_z(v_vrf.z());
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+void PublishLocalization(const double x,const double y,const double z,const double yaw,const double v,const double kappa,const double a) {
|
|
|
|
+ auto localization = std::make_shared<apollo::localization::LocalizationEstimate>();
|
|
|
|
+ FillHeader("SimPerfectControl", localization.get());
|
|
|
|
+ auto *pose = localization->mutable_pose();
|
|
|
|
+
|
|
|
|
+ pose->mutable_position()->set_x(x);
|
|
|
|
+ pose->mutable_position()->set_y(y);
|
|
|
|
+ pose->mutable_position()->set_z(z);
|
|
|
|
+ // Set orientation and heading
|
|
|
|
+ double cur_theta = yaw;
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ Eigen::Quaternion<double> cur_orientation =
|
|
|
|
+ HeadingToQuaternion<double>(cur_theta);
|
|
|
|
+ pose->mutable_orientation()->set_qw(cur_orientation.w());
|
|
|
|
+ pose->mutable_orientation()->set_qx(cur_orientation.x());
|
|
|
|
+ pose->mutable_orientation()->set_qy(cur_orientation.y());
|
|
|
|
+ pose->mutable_orientation()->set_qz(cur_orientation.z());
|
|
|
|
+ pose->set_heading(cur_theta);
|
|
|
|
+
|
|
|
|
+ // Set linear_velocity
|
|
|
|
+ pose->mutable_linear_velocity()->set_x(std::cos(cur_theta) * v);
|
|
|
|
+ pose->mutable_linear_velocity()->set_y(std::sin(cur_theta) * v);
|
|
|
|
+ pose->mutable_linear_velocity()->set_z(0);
|
|
|
|
+
|
|
|
|
+ // Set angular_velocity in both map reference frame and vehicle reference
|
|
|
|
+ // frame
|
|
|
|
+ pose->mutable_angular_velocity()->set_x(0);
|
|
|
|
+ pose->mutable_angular_velocity()->set_y(0);
|
|
|
|
+ pose->mutable_angular_velocity()->set_z(v*kappa);
|
|
|
|
+
|
|
|
|
+ TransformToVRF(pose->angular_velocity(), pose->orientation(),
|
|
|
|
+ pose->mutable_angular_velocity_vrf());
|
|
|
|
+
|
|
|
|
+ // Set linear_acceleration in both map reference frame and vehicle reference
|
|
|
|
+ // frame
|
|
|
|
+ auto *linear_acceleration = pose->mutable_linear_acceleration();
|
|
|
|
+ linear_acceleration->set_x(std::cos(cur_theta) * a);
|
|
|
|
+ linear_acceleration->set_y(std::sin(cur_theta) * a);
|
|
|
|
+ linear_acceleration->set_z(0);
|
|
|
|
+
|
|
|
|
+ TransformToVRF(pose->linear_acceleration(), pose->orientation(),
|
|
|
|
+ pose->mutable_linear_acceleration_vrf());
|
|
|
|
+ localization_writer_->Write(localization);
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+
|
|
template <class T, size_t N>
|
|
template <class T, size_t N>
|
|
constexpr size_t array_size(T (&)[N]) {
|
|
constexpr size_t array_size(T (&)[N]) {
|
|
return N;
|
|
return N;
|
|
@@ -187,6 +281,8 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
|
|
return;
|
|
return;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+ std::cout<<"recv gpsimu."<<std::endl;
|
|
|
|
+
|
|
int64_t timenow = std::chrono::system_clock::now().time_since_epoch().count();
|
|
int64_t timenow = std::chrono::system_clock::now().time_since_epoch().count();
|
|
double seconds = timenow * 1e-9;//gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3;
|
|
double seconds = timenow * 1e-9;//gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3;
|
|
|
|
|
|
@@ -243,12 +339,14 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
|
|
auto *gps_msg = gps->mutable_localization();
|
|
auto *gps_msg = gps->mutable_localization();
|
|
|
|
|
|
// 1. pose xyz
|
|
// 1. pose xyz
|
|
- double x = ins->position().lon();
|
|
|
|
- double y = ins->position().lat();
|
|
|
|
|
|
+ double x = ins->position().lat();
|
|
|
|
+ double y = ins->position().lon();
|
|
x *= DEG_TO_RAD_LOCAL;
|
|
x *= DEG_TO_RAD_LOCAL;
|
|
y *= DEG_TO_RAD_LOCAL;
|
|
y *= DEG_TO_RAD_LOCAL;
|
|
|
|
|
|
pj_transform(wgs84pj_source_, utm_target_, 1, 1, &x, &y, NULL);
|
|
pj_transform(wgs84pj_source_, utm_target_, 1, 1, &x, &y, NULL);
|
|
|
|
+
|
|
|
|
+ std::cout<<" x: "<<x<<" y: "<<y<<std::endl;
|
|
|
|
|
|
gps_msg->mutable_position()->set_x(x);
|
|
gps_msg->mutable_position()->set_x(x);
|
|
gps_msg->mutable_position()->set_y(y);
|
|
gps_msg->mutable_position()->set_y(y);
|
|
@@ -288,6 +386,8 @@ void InitIns(){
|
|
}
|
|
}
|
|
|
|
|
|
int main(int argc, char *argv[]) {
|
|
int main(int argc, char *argv[]) {
|
|
|
|
+ testutm();
|
|
|
|
+ testutm();
|
|
// 初始化一个cyber框架
|
|
// 初始化一个cyber框架
|
|
apollo::cyber::Init(argv[0]);
|
|
apollo::cyber::Init(argv[0]);
|
|
// 创建talker节点
|
|
// 创建talker节点
|
|
@@ -298,17 +398,19 @@ void InitIns(){
|
|
wgs84pj_source_ = pj_init_plus(WGS84_TEXT);
|
|
wgs84pj_source_ = pj_init_plus(WGS84_TEXT);
|
|
utm_target_ = pj_init_plus("+proj=utm +zone=10 +ellps=WGS84 +towgs84=0,0,0,0,0,0,0 +units=m +no_defs");
|
|
utm_target_ = pj_init_plus("+proj=utm +zone=10 +ellps=WGS84 +towgs84=0,0,0,0,0,0,0 +units=m +no_defs");
|
|
|
|
|
|
-
|
|
|
|
|
|
|
|
gps_writer_ = talker_node->CreateWriter<Gps>(FLAGS_gps_topic);
|
|
gps_writer_ = talker_node->CreateWriter<Gps>(FLAGS_gps_topic);
|
|
insstat_writer_ = talker_node->CreateWriter<apollo::drivers::gnss::InsStat>(FLAGS_ins_stat_topic);
|
|
insstat_writer_ = talker_node->CreateWriter<apollo::drivers::gnss::InsStat>(FLAGS_ins_stat_topic);
|
|
corrimu_writer_ = talker_node->CreateWriter<apollo::localization::CorrectedImu>(FLAGS_imu_topic);
|
|
corrimu_writer_ = talker_node->CreateWriter<apollo::localization::CorrectedImu>(FLAGS_imu_topic);
|
|
|
|
+ localization_writer_ = talker_node->CreateWriter<apollo::localization::LocalizationEstimate>(stradcpose);
|
|
// 从节点创建一个Topic,来实现对车速的查看
|
|
// 从节点创建一个Topic,来实现对车速的查看
|
|
// auto talker = talker_node->CreateWriter<Car>("car_speed");
|
|
// auto talker = talker_node->CreateWriter<Car>("car_speed");
|
|
// AINFO << "I'll start telling you the current speed of the car.";
|
|
// AINFO << "I'll start telling you the current speed of the car.";
|
|
|
|
|
|
void * paraw = iv::modulecomm::RegisterRecv("hcp2_gpsimu",ListenGPSIMU);
|
|
void * paraw = iv::modulecomm::RegisterRecv("hcp2_gpsimu",ListenGPSIMU);
|
|
(void)paraw;
|
|
(void)paraw;
|
|
|
|
+
|
|
|
|
+ void * pasend = iv::modulecomm::RegisterSend("b1",1000,1);
|
|
|
|
|
|
std::cout<<" enter talker."<<std::endl;
|
|
std::cout<<" enter talker."<<std::endl;
|
|
//设置初始速度为0,然后速度每秒增加5km/h
|
|
//设置初始速度为0,然后速度每秒增加5km/h
|
|
@@ -319,6 +421,10 @@ void InitIns(){
|
|
//假设车速持续增加
|
|
//假设车速持续增加
|
|
speed += 5;
|
|
speed += 5;
|
|
// talker->Write(msg);
|
|
// talker->Write(msg);
|
|
|
|
+ char str[1000];
|
|
|
|
+ int64_t xTime = std::chrono::system_clock::now().time_since_epoch().count();
|
|
|
|
+ memcpy(str,&xTime,8);
|
|
|
|
+ iv::modulecomm::ModuleSendMsg(pasend,str,30);
|
|
sleep(1);
|
|
sleep(1);
|
|
}
|
|
}
|
|
return 0;
|
|
return 0;
|