在诺耕组合惯导的中转ROS节点中添加UDP广播功能

src/ros/catkin/src/rtk_nav992/src/main.cpp

@@ -4,7 +4,7 @@
 #include <stdlib.h>
 #include <unistd.h>
 #include <fcntl.h>
-#include <termios.h>  
+#include <termios.h>
 #include <thread>
 #include <cmath>
 #include <chrono>
@@ -26,9 +26,7 @@
 #include "gnss_coordinate_convert.h"
 
 static std::string _pose_topic = "/current_pose";
-static std::string _twist_topic =  "/current_velocity";
-
-
+static std::string _twist_topic = "/current_velocity";
 
 static double glon0 = 117.0273054;
 static double glat0 = 39.1238777;
@@ -44,21 +42,22 @@ ros::Subscriber imu_sub;
 double fheading, fLat, fLon, fVel, fPitch, fRoll;
 double fHgt, gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z;
 double ve, vn, vu;
-double adc_x,adc_y,adc_z,adc_w;
+double adc_x, adc_y, adc_z, adc_w;
 bool b_navsatfix = false;
-bool b_twist     = false;
-bool b_imu       = false;
+bool b_twist = false;
+bool b_imu = false;
 
-
-struct Quaternion {
+struct Quaternion
+{
     double w, x, y, z;
 };
 
-struct EulerAngles {
+struct EulerAngles
+{
     double roll, pitch, yaw;
 };
 
-EulerAngles ToEulerAngles(Quaternion q) 
+EulerAngles ToEulerAngles(Quaternion q)
 {
     EulerAngles angles;
 
@@ -82,7 +81,6 @@ EulerAngles ToEulerAngles(Quaternion q)
     return angles;
 }
 
-
 Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
 {
     // Abbreviations for the various angular functions
@@ -104,9 +102,7 @@ Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch
 
 //原文链接：https://blog.csdn.net/xiaoma_bk/article/details/79082629
 
-
-
-void callback_navsatfix(const sensor_msgs::NavSatFixConstPtr& msg)
+void callback_navsatfix(const sensor_msgs::NavSatFixConstPtr &msg)
 {
     ROS_INFO("Received nav992_device/NavSatFix Array");
     fLat = msg->latitude;
@@ -115,7 +111,7 @@ void callback_navsatfix(const sensor_msgs::NavSatFixConstPtr& msg)
     b_navsatfix = true;
 }
 
-void callback_twist(const geometry_msgs::TwistConstPtr& msg)
+void callback_twist(const geometry_msgs::TwistConstPtr &msg)
 {
     ROS_INFO("Received nav992_device/Twist Array");
     vn = msg->linear.x;
@@ -123,7 +119,7 @@ void callback_twist(const geometry_msgs::TwistConstPtr& msg)
     b_twist = true;
 }
 
-void callback_imu(const sensor_msgs::ImuConstPtr& msg)
+void callback_imu(const sensor_msgs::ImuConstPtr &msg)
 {
     ROS_INFO("Received nav992_device/Imu Array");
     adc_x = msg->orientation.x;
@@ -133,10 +129,6 @@ void callback_imu(const sensor_msgs::ImuConstPtr& msg)
     b_imu = true;
 }
 
-
-
-
-
 int main(int argc, char **argv)
 {
     //Initiate ROS
@@ -144,32 +136,30 @@ int main(int argc, char **argv)
     ros::NodeHandle private_nh("~");
     ros::Rate loop_rate(100);
 
-    QUdpSocket  *mudpSocketGPSIMU;
-	    
-    private_nh.getParam("pose_topic",_pose_topic);
-	private_nh.getParam("twist_topic",_twist_topic);
-    std::cout<<"pose_topic  is "<<_pose_topic<<std::endl;
-	std::cout<<"twist_topic : "<<_twist_topic<<std::endl;
+    QUdpSocket *mudpSocketGPSIMU;
 
-    private_nh.getParam("Lon0",glon0);
-    std::cout<<"Lon0 ： "<<glon0<<std::endl;
+    private_nh.getParam("pose_topic", _pose_topic);
+    private_nh.getParam("twist_topic", _twist_topic);
+    std::cout << "pose_topic  is " << _pose_topic << std::endl;
+    std::cout << "twist_topic : " << _twist_topic << std::endl;
 
-    private_nh.getParam("Lat0",glat0);
-    std::cout<<"Lat0 : "<<glat0<<std::endl;
+    private_nh.getParam("Lon0", glon0);
+    std::cout << "Lon0 ： " << glon0 << std::endl;
 
+    private_nh.getParam("Lat0", glat0);
+    std::cout << "Lat0 : " << glat0 << std::endl;
 
-    pose_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_pose_topic,10);
-    twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic,10);
+    pose_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_pose_topic, 10);
+    twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic, 10);
 
-  
     //Topic you want to subscribe
     navsatfix_sub = private_nh.subscribe("/nav992_device/NavSatFix", 1, callback_navsatfix);
-    twist_sub     = private_nh.subscribe("/nav992_device/Twist", 1, callback_twist);
-    imu_sub       = private_nh.subscribe("/nav992_device/Imu", 1, callback_imu);
+    twist_sub = private_nh.subscribe("/nav992_device/Twist", 1, callback_twist);
+    imu_sub = private_nh.subscribe("/nav992_device/Imu", 1, callback_imu);
 
     while (ros::ok())
     {
-	ros::spinOnce();
+        ros::spinOnce();
         double x_o, y_o;
         double x_now, y_now;
         GaussProjCal(glon0, glat0, &x_o, &y_o);
@@ -200,6 +190,16 @@ int main(int argc, char **argv)
         msg.pose.pose.orientation.z = adc_z;
         msg.pose.pose.orientation.w = adc_w;
 
+        Quaternion q;
+        q.w = adc_w;
+        q.x = adc_x;
+        q.y = adc_y;
+        q.z = adc_z;
+
+        EulerAngles angles;
+        angles = ToEulerAngles(q);
+        fheading = angles.yaw;
+
         msg.twist.twist.linear.x = sqrt(pow(vn, 2) + pow(ve, 2));
 
         geometry_msgs::PoseStamped xPose;
@@ -209,7 +209,7 @@ int main(int argc, char **argv)
         xtwist.header.frame_id = "map";
         xtwist.twist = msg.twist.twist;
 
-        if(b_navsatfix==true && b_twist==true && b_imu==true)
+        if (b_navsatfix == true && b_twist == true && b_imu == true)
         {
             pose_pub.publish(xPose);
             twist_pub.publish(xtwist);
@@ -218,6 +218,64 @@ int main(int argc, char **argv)
             b_imu = false;
         }
 
+////////////////////////////////===UDP广播开始===///////////////////////////////////////////
+        unsigned char strSend[72];
+        int i;
+        for (i = 0; i < 72; i++)
+            strSend[i] = 0x0;
+        strSend[0] = 0xE7;
+
+        // strSend[21] = minsstate;
+        // strSend[68] = mstate;
+        strSend[67] = 0;
+        strSend[62] = 62;
+
+        double fV = fLat;
+        fV = fV / (180.0 / M_PI);
+        memcpy(strSend + 23, &fV, 8);
+        fV = fLon;
+        fV = fV / (180.0 / M_PI);
+        memcpy(strSend + 31, &fV, 8);
+        float fF = 0;
+        memcpy(strSend + 39, &fF, 4);
+
+        int iV;
+        char *piV = (char *)&iV;
+        piV++;
+        // fV = mfVe;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+43,piV,3);
+        // fV = mfVn;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+46,piV,3);
+        // fV = mfVu;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+49,piV,3);
+
+        fV = fheading / (180.0 / M_PI);
+        fV = fV * 1000000.0;
+        iV = (int)fV;
+        iV = iV * 256;
+        memcpy(strSend + 52, piV, 3);
+        // fV = mfPitch/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+55,piV,3);
+        // fV = mfRoll/(180.0/M_PI);fV = fV*1000000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+58,piV,3);
+
+        // double facc_x,facc_y,facc_z;
+        // facc_x = mfAcc_x;
+        // facc_y = mfAcc_y;
+        // facc_z = mfAcc_z;
+
+        // fV = facc_x;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+3,piV,3);
+        // fV = facc_y;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+6,piV,3);
+        // fV = facc_z;fV = fV*10000.0;iV = (int)fV;iV = iV*256;memcpy(strSend+9,piV,3);
+
+        // fV = mfgyro_x;fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+12,piV,3);
+        // fV = mfgyro_y;fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+15,piV,3);
+        // fV = mfgyro_z;fV = fV*100000.0*M_PI/180.0;iV = (int)fV;iV = iV*256;memcpy(strSend+18,piV,3);
+
+        char c;
+        c = strSend[0];
+        for (i = 1; i < 71; i++)
+            c = c + strSend[i];
+        strSend[71] = c;
+
+        mudpSocketGPSIMU->writeDatagram((char *)strSend, 72, QHostAddress::Broadcast, 3000);
+
+//////////////////////////////////////===UDP广播结束===////////////////////////////////////////////////
 
         //ros::spin();
         loop_rate.sleep();