|
|
@@ -0,0 +1,223 @@
|
|
|
+#include "ros/ros.h"
|
|
|
+
|
|
|
+#include <stdio.h>
|
|
|
+#include <stdlib.h>
|
|
|
+#include <unistd.h>
|
|
|
+#include <fcntl.h>
|
|
|
+#include <termios.h>
|
|
|
+#include <thread>
|
|
|
+#include <cmath>
|
|
|
+#include <chrono>
|
|
|
+#include <stdint.h>
|
|
|
+#include <string>
|
|
|
+
|
|
|
+#include <nav_msgs/Odometry.h>
|
|
|
+#include <geometry_msgs/PointStamped.h>
|
|
|
+#include <geometry_msgs/TwistStamped.h>
|
|
|
+#include <geometry_msgs/PoseStamped.h>
|
|
|
+#include <sensor_msgs/NavSatFix.h>
|
|
|
+#include <sensor_msgs/TimeReference.h>
|
|
|
+#include <sensor_msgs/Imu.h>
|
|
|
+#include <geometry_msgs/Twist.h>
|
|
|
+
|
|
|
+#include "gnss_coordinate_convert.h"
|
|
|
+
|
|
|
+static std::string _pose_topic = "/current_pose";
|
|
|
+static std::string _twist_topic = "/current_velocity";
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+static double glon0 = 117.0273054;
|
|
|
+static double glat0 = 39.1238777;
|
|
|
+static double ghead0 = 0;
|
|
|
+
|
|
|
+ros::Publisher pose_pub;
|
|
|
+ros::Publisher twist_pub;
|
|
|
+
|
|
|
+ros::Subscriber navsatfix_sub;
|
|
|
+ros::Subscriber twist_sub;
|
|
|
+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;
|
|
|
+bool b_navsatfix = false;
|
|
|
+bool b_twist = false;
|
|
|
+bool b_imu = false;
|
|
|
+
|
|
|
+
|
|
|
+struct Quaternion {
|
|
|
+ double w, x, y, z;
|
|
|
+};
|
|
|
+
|
|
|
+struct EulerAngles {
|
|
|
+ double roll, pitch, yaw;
|
|
|
+};
|
|
|
+
|
|
|
+EulerAngles ToEulerAngles(Quaternion q)
|
|
|
+{
|
|
|
+ EulerAngles angles;
|
|
|
+
|
|
|
+ // roll (x-axis rotation)
|
|
|
+ double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
|
|
|
+ double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
|
|
|
+ angles.roll = std::atan2(sinr_cosp, cosr_cosp);
|
|
|
+
|
|
|
+ // pitch (y-axis rotation)
|
|
|
+ double sinp = 2 * (q.w * q.y - q.z * q.x);
|
|
|
+ if (std::abs(sinp) >= 1)
|
|
|
+ angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
|
|
|
+ else
|
|
|
+ angles.pitch = std::asin(sinp);
|
|
|
+
|
|
|
+ // yaw (z-axis rotation)
|
|
|
+ double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
|
|
|
+ double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
|
|
|
+ angles.yaw = std::atan2(siny_cosp, cosy_cosp);
|
|
|
+
|
|
|
+ return angles;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
|
|
|
+{
|
|
|
+ // Abbreviations for the various angular functions
|
|
|
+ double cy = cos(yaw * 0.5);
|
|
|
+ double sy = sin(yaw * 0.5);
|
|
|
+ double cp = cos(pitch * 0.5);
|
|
|
+ double sp = sin(pitch * 0.5);
|
|
|
+ double cr = cos(roll * 0.5);
|
|
|
+ double sr = sin(roll * 0.5);
|
|
|
+
|
|
|
+ Quaternion q;
|
|
|
+ q.w = cy * cp * cr + sy * sp * sr;
|
|
|
+ q.x = cy * cp * sr - sy * sp * cr;
|
|
|
+ q.y = sy * cp * sr + cy * sp * cr;
|
|
|
+ q.z = sy * cp * cr - cy * sp * sr;
|
|
|
+
|
|
|
+ return q;
|
|
|
+}
|
|
|
+
|
|
|
+//原文链接：https://blog.csdn.net/xiaoma_bk/article/details/79082629
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void callback_navsatfix(const sensor_msgs::NavSatFixConstPtr& msg)
|
|
|
+{
|
|
|
+ ROS_INFO("Received nav992_device/NavSatFix Array");
|
|
|
+ fLat = msg->latitude;
|
|
|
+ fLon = msg->longitude;
|
|
|
+ fHgt = msg->altitude;
|
|
|
+ b_navsatfix = true;
|
|
|
+}
|
|
|
+
|
|
|
+void callback_twist(const geometry_msgs::TwistConstPtr& msg)
|
|
|
+{
|
|
|
+ ROS_INFO("Received nav992_device/Twist Array");
|
|
|
+ vn = msg->linear.x;
|
|
|
+ ve = msg->linear.y;
|
|
|
+ b_twist = true;
|
|
|
+}
|
|
|
+
|
|
|
+void callback_imu(const sensor_msgs::ImuConstPtr& msg)
|
|
|
+{
|
|
|
+ ROS_INFO("Received nav992_device/Imu Array");
|
|
|
+ adc_x = msg->orientation.x;
|
|
|
+ adc_y = msg->orientation.y;
|
|
|
+ adc_z = msg->orientation.z;
|
|
|
+ adc_w = msg->orientation.w;
|
|
|
+ b_imu = true;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+int main(int argc, char **argv)
|
|
|
+{
|
|
|
+ //Initiate ROS
|
|
|
+ ros::init(argc, argv, "pilottoros");
|
|
|
+ ros::NodeHandle private_nh("~");
|
|
|
+ ros::Rate loop_rate(100);
|
|
|
+
|
|
|
+
|
|
|
+ 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("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);
|
|
|
+
|
|
|
+
|
|
|
+ //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);
|
|
|
+
|
|
|
+ while (ros::ok())
|
|
|
+ {
|
|
|
+
|
|
|
+ double x_o, y_o;
|
|
|
+ double x_now, y_now;
|
|
|
+ GaussProjCal(glon0, glat0, &x_o, &y_o);
|
|
|
+ GaussProjCal(fLon, fLat, &x_now, &y_now);
|
|
|
+ double rel_x = x_now - x_o;
|
|
|
+ double rel_y = y_now - y_o;
|
|
|
+ double rel_z = fHgt;
|
|
|
+
|
|
|
+ // double pitch, roll, yaw;
|
|
|
+ // pitch = fPitch * M_PI / 180.0;
|
|
|
+ // roll = fRoll * M_PI / 180.0;
|
|
|
+ // double hdg_o = (90 - ghead0) * M_PI / 180.0;
|
|
|
+ // yaw = (90 - fheading) * M_PI / 180.0; // - hdg_o;
|
|
|
+ // while (yaw < 0)
|
|
|
+ // yaw = yaw + 2.0 * M_PI;
|
|
|
+ // while (yaw >= (2.0 * M_PI))
|
|
|
+ // yaw = yaw - 2.0 * M_PI;
|
|
|
+ // Quaternion quat = ToQuaternion(yaw, pitch, roll);
|
|
|
+
|
|
|
+ nav_msgs::Odometry msg;
|
|
|
+
|
|
|
+ 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 = adc_x;
|
|
|
+ msg.pose.pose.orientation.y = adc_y;
|
|
|
+ msg.pose.pose.orientation.z = adc_z;
|
|
|
+ msg.pose.pose.orientation.w = adc_w;
|
|
|
+
|
|
|
+ msg.twist.twist.linear.x = sqrt(pow(vn, 2) + pow(ve, 2));
|
|
|
+
|
|
|
+ geometry_msgs::PoseStamped xPose;
|
|
|
+ xPose.header.frame_id = "map";
|
|
|
+ xPose.pose = msg.pose.pose;
|
|
|
+ geometry_msgs::TwistStamped xtwist;
|
|
|
+ xtwist.header.frame_id = "map";
|
|
|
+ xtwist.twist = msg.twist.twist;
|
|
|
+
|
|
|
+ if(b_navsatfix==true && b_twist==true && b_imu==true)
|
|
|
+ {
|
|
|
+ pose_pub.publish(xPose);
|
|
|
+ twist_pub.publish(xtwist);
|
|
|
+ b_navsatfix = false;
|
|
|
+ b_twist = false;
|
|
|
+ b_imu = false;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ ros::spin();
|
|
|
+ loop_rate.sleep();
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
fujiankuan
