|
@@ -92,10 +92,16 @@ pilot_autoware_bridge::pilot_autoware_bridge() : Node("pilot_autoware_bridge")
|
|
|
mptimer= create_wall_timer(100ms,std::bind(&pilot_autoware_bridge::callbackTimer, this));
|
|
|
mptimerGPS = create_wall_timer(10ms,std::bind(&pilot_autoware_bridge::callbackTimerGPS, this));
|
|
|
|
|
|
+ pub_control_mode_report_ =
|
|
|
+ create_publisher<ControlModeReport>("/vehicle/status/control_mode",durable_qos);
|
|
|
+
|
|
|
if(mbTestSim)
|
|
|
{
|
|
|
mptimerTestSim = create_wall_timer(10ms,std::bind(&pilot_autoware_bridge::callbackTimerTestSim,this));
|
|
|
}
|
|
|
+
|
|
|
+ // control mode
|
|
|
+ current_control_mode_.mode = ControlModeReport::AUTONOMOUS;
|
|
|
// 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);
|
|
|
|
|
@@ -419,6 +425,7 @@ void pilot_autoware_bridge::callbackTimer()
|
|
|
{
|
|
|
// std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" testpose1. "<<std::endl;
|
|
|
|
|
|
+ publish_control_mode_report();
|
|
|
|
|
|
if(mbcmdupdate == true)
|
|
|
{
|
|
@@ -864,3 +871,9 @@ void pilot_autoware_bridge::ConvertObjAndPub(iv::lidar::objectarray * pobjarray)
|
|
|
// pointcloud_pub_->publish(output_pointcloud_msg);
|
|
|
}
|
|
|
|
|
|
+void pilot_autoware_bridge::publish_control_mode_report()
|
|
|
+{
|
|
|
+ current_control_mode_.stamp = get_clock()->now();
|
|
|
+ pub_control_mode_report_->publish(current_control_mode_);
|
|
|
+}
|
|
|
+
|