|
|
@@ -0,0 +1,571 @@
|
|
|
+#include "ros/ros.h"
|
|
|
+
|
|
|
+#include <stdio.h>
|
|
|
+#include <stdlib.h>
|
|
|
+#include <unistd.h>
|
|
|
+#include <fcntl.h>
|
|
|
+//#include <asm/termios.h>
|
|
|
+#include <termios.h>
|
|
|
+
|
|
|
+#include <QMutex>
|
|
|
+#include <QSerialPort>
|
|
|
+
|
|
|
+#include <nav_msgs/Odometry.h>
|
|
|
+#include <geometry_msgs/PointStamped.h>
|
|
|
+#include <geometry_msgs/TwistStamped.h>
|
|
|
+#include <geometry_msgs/PoseStamped.h>
|
|
|
+
|
|
|
+#include <tf/tf.h>
|
|
|
+#include <tf/transform_broadcaster.h>
|
|
|
+#include <tf/transform_datatypes.h>
|
|
|
+#include <tf/transform_listener.h>
|
|
|
+
|
|
|
+#include <thread>
|
|
|
+
|
|
|
+#include "gnss_coordinate_convert.h"
|
|
|
+
|
|
|
+static std::string _pose_topic = "/current_pose";
|
|
|
+static std::string _twist_topic = "/current_velocity";
|
|
|
+
|
|
|
+static std::string gstrserialportname = "/dev/ttyUSB0";
|
|
|
+
|
|
|
+static int gnBaudRate = 230400;
|
|
|
+static double glon0 = 117.0273054;
|
|
|
+static double glat0 = 39.1238777;
|
|
|
+static double ghead0 = 0;
|
|
|
+
|
|
|
+ros::Publisher pose_pub;
|
|
|
+ros::Publisher twist_pub;
|
|
|
+
|
|
|
+QString mstrHCP2;
|
|
|
+
|
|
|
+void SerialGPSDecodeSen(QString strsen);
|
|
|
+
|
|
|
+#include <cmath>
|
|
|
+
|
|
|
+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
|
|
|
+
|
|
|
+static void SerialGPSDecode()
|
|
|
+{
|
|
|
+ int xpos = mstrHCP2.indexOf('\n');
|
|
|
+ // std::cout<<" xpos : "<<xpos<<std::endl;
|
|
|
+ if (xpos >= 0)
|
|
|
+ {
|
|
|
+
|
|
|
+ QString xsen = mstrHCP2.left(xpos + 1);
|
|
|
+ // std::cout<<" decode "<<std::endl;
|
|
|
+ SerialGPSDecodeSen(xsen);
|
|
|
+ mstrHCP2.remove(0, xpos + 1);
|
|
|
+ SerialGPSDecode();
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+static bool checknmeasen(const char *strsen, const unsigned int nlen)
|
|
|
+{
|
|
|
+ if (nlen < 4)
|
|
|
+ return false;
|
|
|
+
|
|
|
+ int i;
|
|
|
+ char check;
|
|
|
+ int nstarpos = -1;
|
|
|
+ check = strsen[1] ^ strsen[2];
|
|
|
+ for (i = 3; i < nlen; i++)
|
|
|
+ {
|
|
|
+ if (strsen[i] == '*')
|
|
|
+ {
|
|
|
+ nstarpos = i;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ check = check ^ strsen[i];
|
|
|
+ }
|
|
|
+ if (nstarpos < 0)
|
|
|
+ return false;
|
|
|
+ if (nstarpos > (nlen - 3))
|
|
|
+ return false;
|
|
|
+ char strcheck[3];
|
|
|
+ int ncheck = check;
|
|
|
+ snprintf(strcheck, 3, "%02X", ncheck);
|
|
|
+ char strsencheck[3];
|
|
|
+ strsencheck[2] = 0;
|
|
|
+ strsencheck[0] = strsen[nstarpos + 1];
|
|
|
+ strsencheck[1] = strsen[nstarpos + 2];
|
|
|
+ if (strncmp(strcheck, strsencheck, 2) == 0)
|
|
|
+ {
|
|
|
+ // qDebug(" r sen is %s",strsen);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+ // std::cout<<" check fail. "<<std::endl;
|
|
|
+ // qDebug(" sen is %s",strsen);
|
|
|
+ return false;
|
|
|
+}
|
|
|
+
|
|
|
+void SerialGPSDecodeSen(QString strsen)
|
|
|
+{
|
|
|
+
|
|
|
+ static tf::TransformBroadcaster br;
|
|
|
+ tf::Transform transform;
|
|
|
+ tf::Quaternion current_q;
|
|
|
+
|
|
|
+ QStringList strlistrmc;
|
|
|
+ strlistrmc = strsen.split(",");
|
|
|
+
|
|
|
+ if (strlistrmc.size() < 23)
|
|
|
+ return;
|
|
|
+ if (strlistrmc.at(0) != "$GPCHC")
|
|
|
+ return;
|
|
|
+
|
|
|
+ if (!checknmeasen(strsen.toLatin1().data(), strsen.length()))
|
|
|
+ {
|
|
|
+ int nPos = strsen.indexOf('$', 10);
|
|
|
+ if (nPos >= 0)
|
|
|
+ {
|
|
|
+ QString strnewsen = strsen.right(strsen.size() - nPos);
|
|
|
+ // qDebug("new sen is %s",strnewsen.toLatin1().data());
|
|
|
+ SerialGPSDecodeSen(strnewsen);
|
|
|
+ }
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ double fheading, fLat, fLon, fVel, fPitch, fRoll;
|
|
|
+ double fHgt, gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z;
|
|
|
+ int nsv1, nsv2;
|
|
|
+ int gpsweek, gpstime;
|
|
|
+ int insstate, rtkstate;
|
|
|
+ QString strx = strlistrmc.at(3);
|
|
|
+ fheading = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(1);
|
|
|
+ gpsweek = strx.toInt();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(2);
|
|
|
+ gpstime = strx.toInt();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(12);
|
|
|
+ fLat = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(13);
|
|
|
+ fLon = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(14);
|
|
|
+ fHgt = strx.toDouble();
|
|
|
+
|
|
|
+ double ve, vn, vu;
|
|
|
+ strx = strlistrmc.at(15);
|
|
|
+ ve = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(16);
|
|
|
+ vn = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(17);
|
|
|
+ vu = strx.toDouble();
|
|
|
+
|
|
|
+ // fVel = sqrt(ve*ve + vn*vn + vu*vu);
|
|
|
+ strx = strlistrmc.at(18);
|
|
|
+ fVel = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(4);
|
|
|
+ fPitch = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(5);
|
|
|
+ fRoll = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(6);
|
|
|
+ gyro_x = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(7);
|
|
|
+ gyro_y = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(8);
|
|
|
+ gyro_z = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(9);
|
|
|
+ acc_x = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(10);
|
|
|
+ acc_y = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(11);
|
|
|
+ acc_z = strx.toDouble();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(19);
|
|
|
+ nsv1 = strx.toInt();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(20);
|
|
|
+ nsv2 = strx.toInt();
|
|
|
+
|
|
|
+ strx = strlistrmc.at(21);
|
|
|
+ char strstate[3];
|
|
|
+ strstate[2] = 0;
|
|
|
+ if (strx.size() >= 2)
|
|
|
+ {
|
|
|
+
|
|
|
+ memcpy(strstate, strx.data(), 2);
|
|
|
+ // qDebug(strstate);
|
|
|
+ char xstate;
|
|
|
+ xstate = strstate[0];
|
|
|
+ rtkstate = 4;
|
|
|
+ int xs;
|
|
|
+ if (xstate == '0')
|
|
|
+ xs = 0;
|
|
|
+ if (xstate == '1')
|
|
|
+ xs = 3;
|
|
|
+ if (xstate == '2')
|
|
|
+ xs = 4;
|
|
|
+ if (xstate == '3')
|
|
|
+ xs = 8;
|
|
|
+ if (xstate == '4')
|
|
|
+ xs = 6;
|
|
|
+ if (xstate == '5')
|
|
|
+ xs = 5;
|
|
|
+ if (xstate == '6')
|
|
|
+ xs = 3;
|
|
|
+ if (xstate == '7')
|
|
|
+ xs = 4;
|
|
|
+ if (xstate == '8')
|
|
|
+ xs = 5;
|
|
|
+ if (xstate == '9')
|
|
|
+ xs = 5;
|
|
|
+
|
|
|
+ rtkstate = xs;
|
|
|
+ // if(mstate == 0)minsstate = 0;
|
|
|
+ // else minsstate = 4;
|
|
|
+
|
|
|
+ xstate = strstate[1];
|
|
|
+ if (xstate == '0')
|
|
|
+ insstate = 3;
|
|
|
+ else
|
|
|
+ insstate = 4;
|
|
|
+ if (rtkstate == 0)
|
|
|
+ insstate = 3;
|
|
|
+ }
|
|
|
+
|
|
|
+ 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 = quat.x;
|
|
|
+ msg.pose.pose.orientation.y = quat.y;
|
|
|
+ msg.pose.pose.orientation.z = quat.z;
|
|
|
+ msg.pose.pose.orientation.w = quat.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;
|
|
|
+
|
|
|
+ current_q.setRPY(roll, pitch, yaw);
|
|
|
+
|
|
|
+ transform.setOrigin(tf::Vector3(rel_x, rel_y, rel_z));
|
|
|
+ // transform.setOrigin(tf::Vector3(2.0*sin(ros::Time::now().toSec()),2.0*cos(ros::Time::now().toSec()),0.0));
|
|
|
+ transform.setRotation(current_q);
|
|
|
+
|
|
|
+ ros::Time current_scan_time = ros::Time::now();
|
|
|
+ br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
|
|
|
+
|
|
|
+ pose_pub.publish(xPose);
|
|
|
+ twist_pub.publish(xtwist);
|
|
|
+
|
|
|
+ // std::cout<<"current pose"<<std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+static void set_baudrate(struct termios *opt, unsigned int baudrate)
|
|
|
+{
|
|
|
+ cfsetispeed(opt, baudrate);
|
|
|
+ cfsetospeed(opt, baudrate);
|
|
|
+}
|
|
|
+
|
|
|
+static void set_data_bit(struct termios *opt, unsigned int databit)
|
|
|
+{
|
|
|
+ opt->c_cflag &= ~CSIZE;
|
|
|
+ switch (databit)
|
|
|
+ {
|
|
|
+ case 8:
|
|
|
+ opt->c_cflag |= CS8;
|
|
|
+ break;
|
|
|
+ case 7:
|
|
|
+ opt->c_cflag |= CS7;
|
|
|
+ break;
|
|
|
+ case 6:
|
|
|
+ opt->c_cflag |= CS6;
|
|
|
+ break;
|
|
|
+ case 5:
|
|
|
+ opt->c_cflag |= CS5;
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ opt->c_cflag |= CS8;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+static void set_parity(struct termios *opt, char parity)
|
|
|
+{
|
|
|
+ switch (parity)
|
|
|
+ {
|
|
|
+ case 'N': /* no parity check */
|
|
|
+ opt->c_cflag &= ~PARENB;
|
|
|
+ break;
|
|
|
+ case 'E': /* even */
|
|
|
+ opt->c_cflag |= PARENB;
|
|
|
+ opt->c_cflag &= ~PARODD;
|
|
|
+ break;
|
|
|
+ case 'O': /* odd */
|
|
|
+ opt->c_cflag |= PARENB;
|
|
|
+ opt->c_cflag |= ~PARODD;
|
|
|
+ break;
|
|
|
+ default: /* no parity check */
|
|
|
+ opt->c_cflag &= ~PARENB;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+static void set_stopbit(struct termios *opt, const char *stopbit)
|
|
|
+{
|
|
|
+ if (0 == strcmp(stopbit, "1"))
|
|
|
+ {
|
|
|
+ opt->c_cflag &= ~CSTOPB; /* 1 stop bit */
|
|
|
+ }
|
|
|
+ else if (0 == strcmp(stopbit, "1"))
|
|
|
+ {
|
|
|
+ opt->c_cflag &= ~CSTOPB; /* 1.5 stop bit */
|
|
|
+ }
|
|
|
+ else if (0 == strcmp(stopbit, "2"))
|
|
|
+ {
|
|
|
+ opt->c_cflag |= CSTOPB; /* 2 stop bits */
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ opt->c_cflag &= ~CSTOPB; /* 1 stop bit */
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+//https://blog.csdn.net/morixinguan/article/details/80898172
|
|
|
+//串口设置
|
|
|
+int set_port_attr(
|
|
|
+ int fd,
|
|
|
+ int baudrate, // B1200 B2400 B4800 B9600 .. B115200
|
|
|
+ int databit, // 5, 6, 7, 8
|
|
|
+ const char *stopbit, // "1", "1.5", "2"
|
|
|
+ char parity, // N(o), O(dd), E(ven)
|
|
|
+ int vtime,
|
|
|
+ int vmin)
|
|
|
+{
|
|
|
+ struct termios opt;
|
|
|
+ tcgetattr(fd, &opt);
|
|
|
+ //设置波特率
|
|
|
+ set_baudrate(&opt, baudrate);
|
|
|
+ opt.c_cflag |= CLOCAL | CREAD; /* | CRTSCTS */
|
|
|
+ //设置数据位
|
|
|
+ set_data_bit(&opt, databit);
|
|
|
+ //设置校验位
|
|
|
+ set_parity(&opt, parity);
|
|
|
+ //设置停止位
|
|
|
+ set_stopbit(&opt, stopbit);
|
|
|
+ //其它设置
|
|
|
+ opt.c_oflag = 0;
|
|
|
+ opt.c_lflag |= 0;
|
|
|
+ opt.c_oflag &= ~OPOST;
|
|
|
+ opt.c_cc[VTIME] = vtime;
|
|
|
+ opt.c_cc[VMIN] = vmin;
|
|
|
+ tcflush(fd, TCIFLUSH);
|
|
|
+ return (tcsetattr(fd, TCSANOW, &opt));
|
|
|
+}
|
|
|
+
|
|
|
+void ThreadRecvRTK()
|
|
|
+{
|
|
|
+ std::cout << " enter thread " << std::endl;
|
|
|
+ QSerialPort *m_serialPort_GPS;
|
|
|
+ m_serialPort_GPS = new QSerialPort();
|
|
|
+ m_serialPort_GPS->setPortName(gstrserialportname.data());
|
|
|
+ m_serialPort_GPS->setBaudRate(230400);
|
|
|
+ m_serialPort_GPS->setParity(QSerialPort::NoParity);
|
|
|
+ m_serialPort_GPS->setDataBits(QSerialPort::Data8);
|
|
|
+ m_serialPort_GPS->setStopBits(QSerialPort::OneStop);
|
|
|
+ m_serialPort_GPS->setFlowControl(QSerialPort::NoFlowControl);
|
|
|
+ m_serialPort_GPS->setReadBufferSize(0);
|
|
|
+ // m_serialPort_GPS->open(QIODevice::ReadWrite);
|
|
|
+ // m_serialPort_GPS->close();
|
|
|
+
|
|
|
+ int fd;
|
|
|
+ int len, i, ret;
|
|
|
+
|
|
|
+ fd = open(gstrserialportname.data(), O_RDWR | O_NOCTTY);
|
|
|
+ if (fd < 0)
|
|
|
+ {
|
|
|
+ perror(gstrserialportname.data());
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ set_port_attr(fd, B230400, 8, "1", 'N', 0, 0);
|
|
|
+
|
|
|
+ char strbuf[1000];
|
|
|
+ bool bSerialPortOpen = true;
|
|
|
+
|
|
|
+ /*
|
|
|
+ bool bSerialPortOpen = false;
|
|
|
+ std::cout<<" open "<<std::endl;
|
|
|
+ if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
|
|
|
+ {
|
|
|
+ bSerialPortOpen = true;
|
|
|
+ std::cout<<" succ "<<std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::cout<<" Open Port "<<gstrserialportname<<" Fail."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ */
|
|
|
+
|
|
|
+ while (bSerialPortOpen)
|
|
|
+ {
|
|
|
+ // QByteArray ba = m_serialPort_GPS->readAll();
|
|
|
+
|
|
|
+ len = read(fd, strbuf, 999);
|
|
|
+ if (len > 0)
|
|
|
+ {
|
|
|
+ strbuf[len] = 0;
|
|
|
+ // std::cout<<strbuf<<std::endl;
|
|
|
+ // std::cout<<" ba len : "<<len<<std::endl;
|
|
|
+ mstrHCP2.append(strbuf);
|
|
|
+ SerialGPSDecode();
|
|
|
+ // mnNotRecvCount = 0;
|
|
|
+ //Decode
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
|
+ // std::cout<<" no data"<<std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+int main(int argc, char *argv[])
|
|
|
+{
|
|
|
+ ros::init(argc, argv, "pilottoros");
|
|
|
+ // ros::NodeHandle n;
|
|
|
+ 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("PortName", gstrserialportname);
|
|
|
+ std::cout << "Port Name : " << gstrserialportname << std::endl;
|
|
|
+
|
|
|
+ private_nh.getParam("BaudRate", gnBaudRate);
|
|
|
+ std::cout << "BaudRate : " << gnBaudRate << 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);
|
|
|
+
|
|
|
+ // static tf::TransformBroadcaster br;
|
|
|
+ // tf::Transform transform;
|
|
|
+ // tf::Quaternion current_q;
|
|
|
+
|
|
|
+ // double timestamp_last = ros::Time::now().toSec();
|
|
|
+
|
|
|
+ // while (ros::ok)
|
|
|
+ // {
|
|
|
+ // if (ros::Time::now().toSec() - timestamp_last > 0.1)
|
|
|
+ // {
|
|
|
+ // current_q.setRPY(0.0, 0.0, 0.0);
|
|
|
+
|
|
|
+ // transform.setOrigin(tf::Vector3(2.0 * sin(ros::Time::now().toSec()), 2.0 * cos(ros::Time::now().toSec()), 0.0));
|
|
|
+ // transform.setRotation(current_q);
|
|
|
+
|
|
|
+ // ros::Time current_scan_time = ros::Time::now();
|
|
|
+ // br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
|
|
|
+ // timestamp_last = ros::Time::now().toSec();
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+
|
|
|
+ std::thread *pthread = new std::thread(ThreadRecvRTK);
|
|
|
+
|
|
|
+ ros::spin();
|
|
|
+}
|
fujiankuan