项目实训 - 智能车系统 - 第四周记录
日期：3.14 – 3.20
项目进度
本周工作进展：
- 进行TF部分的部分移植（本周主要完成了TF相关类型，函数的处理）
1、针对项目进行TF总体分析
本周本来打算继续按照之前的思路进行话题的移植，但是在移植的过程中发现，需要移植的几个话题（lio_sam/mapping/odometry_Incrememt lio_sam/mapping/odometry等），内部实现使用了ros的TF，包括TF相关类型，TF坐标系转换关系的发布和订阅，没有办法进行简单的话题移植，需要先自己实现一套TF机制。于是将本周的任务定为TF的移植。
在开始的分析中，认为TF的修改部分为一下几个部分：
-
tf命名空间
-
对相关函数的修改
- TF自身的函数
- TF与ros相关的函数
- TF与其他库的函数
-
话题发布的修改
- 搞清楚TF发布的话题的内容
对于每个节点：
- feature：没有相关内容
- image：有
- map：有
- imu：有
根据上图可以看到与TF话题相关的节点。
可以发现：涉及到TF通信的节点有2个：map和imu
image涉及到tf，但是不涉及tf通信，可以单独先处理
2、从image节点出发，重写TF相关的类和函数
先在image、imu、map节点的源文件中，找出与TF相关的代码：
image：
tf::Quaternion orientation;
tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
double roll, pitch, yaw;
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
- tf::Quaternion orientation;
类型 - tf::quaternionMsgToTF
函数 - tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
函数
imu：
tf::TransformListener tfListener;
tf::StampedTransform lidar2Baselink;
if(lidarFrame != baselinkFrame)
{
try
{
tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
}
Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
{
double x, y, z, roll, pitch, yaw;
x = odom.pose.pose.position.x;
y = odom.pose.pose.position.y;
z = odom.pose.pose.position.z;
tf::Quaternion orientation;
tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
return pcl::getTransformation(x, y, z, roll, pitch, yaw);
}
static tf::TransformBroadcaster tfMap2Odom;
static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
// publish tf
static tf::TransformBroadcaster tfOdom2BaseLink;
tf::Transform tCur;
tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
if(lidarFrame != baselinkFrame)
tCur = tCur * lidar2Baselink;
tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
tfOdom2BaseLink.sendTransform(odom_2_baselink);
map
if (cloudInfo.imuAvailable == true)
{
if (std::abs(cloudInfo.imuPitchInit) < 1.4)
{
double imuWeight = imuRPYWeight;
tf::Quaternion imuQuaternion;
tf::Quaternion transformQuaternion;
double rollMid, pitchMid, yawMid;
// slerp roll
transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
transformTobeMapped[0] = rollMid;
// slerp pitch
transformQuaternion.setRPY(0, transformTobeMapped[1], 0);
imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
transformTobeMapped[1] = pitchMid;
}
}
tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
void publishOdometry()
{
// Publish odometry for ROS (global)
nav_msgs::Odometry laserOdometryROS;
// laserOdometryROS.header.stamp = timeLaserInfoStamp;
laserOdometryROS.header.stamp.sec = timeLaserInfoStamp.sec;
laserOdometryROS.header.stamp.nsec = timeLaserInfoStamp.nsec;
laserOdometryROS.header.frame_id = odometryFrame;
laserOdometryROS.child_frame_id = "odom_mapping";
laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
pubLaserOdometryGlobal.publish(laserOdometryROS);
//std::cout<<"map1 pub frame_id: "<<laserOdometryROS.header.frame_id<<endl;
// Publish TF
static tf::TransformBroadcaster br;
tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
ros::Time temp_timeLaserInfoStamp;
temp_timeLaserInfoStamp.sec = timeLaserInfoStamp.sec;
temp_timeLaserInfoStamp.nsec = timeLaserInfoStamp.nsec;
//发布tf的问题 /
tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, temp_timeLaserInfoStamp, odometryFrame, "lidar_link");
br.sendTransform(trans_odom_to_lidar);
// Publish odometry for ROS (incremental)
static bool lastIncreOdomPubFlag = false;
static nav_msgs::Odometry laserOdomIncremental; // incremental odometry msg
//定义了一个静态的消息类型 lio_sam/mapping/odometry_Incrememtal、、、、、、、、、、、、、、、、、、、、、、、、、
static nav_msgs::Odometry laserOdomIncremental_sharemem;
static Eigen::Affine3f increOdomAffine; // incremental odometry in affine
if (lastIncreOdomPubFlag == false)
{
lastIncreOdomPubFlag = true;
laserOdomIncremental = laserOdometryROS;//laserOdometryROS也是需要发布的类型 所以暂时还不能改
//pub_change_nav_msgs_Odometry(laserOdomIncremental, laserOdomIncremental_sharemem);//
increOdomAffine = trans2Affine3f(transformTobeMapped);
} else {
Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;
increOdomAffine = increOdomAffine * affineIncre;
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles (increOdomAffine, x, y, z, roll, pitch, yaw);
if (cloudInfo.imuAvailable == true)
{
if (std::abs(cloudInfo.imuPitchInit) < 1.4)
{
double imuWeight = 0.1;
tf::Quaternion imuQuaternion;
tf::Quaternion transformQuaternion;
double rollMid, pitchMid, yawMid;
// slerp roll
transformQuaternion.setRPY(roll, 0, 0);
imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
roll = rollMid;
// slerp pitch
transformQuaternion.setRPY(0, pitch, 0);
imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
pitch = pitchMid;
}
}
//laserOdomIncremental.header.stamp = timeLaserInfoStamp;
laserOdomIncremental.header.stamp.sec = timeLaserInfoStamp.sec;
laserOdomIncremental.header.stamp.nsec = timeLaserInfoStamp.nsec;
laserOdomIncremental.header.frame_id = odometryFrame;
laserOdomIncremental.child_frame_id = "odom_mapping";
//pub_change_nav_msgs_Odometry(laserOdomIncremental,laserOdomIncremental_sharemem)
pubLaserOdometryIncremental.publish(laserOdomIncremental);
//std::cout<<"map2 pub frame_id: "<<laserOdomIncremental.header.frame_id<<endl;
laserOdomIncremental.pose.pose.position.x = x;
laserOdomIncremental.pose.pose.position.y = y;
laserOdomIncremental.pose.pose.position.z = z;
laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
if (isDegenerate)
laserOdomIncremental.pose.covariance[0] = 1;
else
laserOdomIncremental.pose.covariance[0] = 0;
}
//类型转换
//addMessage_nav_msgs_Odometry((nav_msgs_Odometry_List*)pubLaserOdometryIncremental_sharemem->point, laserOdomIncremental_sharemem);
}
根据提取出的代码，在tf/tf.h文件中定义相关的类和函数。
quaternion类（四元数）。
通常采用四元数来表示一个物体的旋转信息
/**
* @brief Quaternion
* 四元数
*/
class Quaternion
{
public:
/*
四元数的基本成员变量
*/
double x;
double y;
double z;
double w;
Quaternion(){}
/*
* 构造函数
*/
Quaternion(double tx,double ty,double tz,double tw)
{
x = tx;
y = ty;
z = tz;
w = tw;
}
/*
* 构造函数，通过其他四元数
*/
Quaternion(const Quaternion& other)
{
x = other.x;
y = other.y;
z = other.z;
w = other.w;
}
void setValue(double tx, double ty, double tz, double tw)
{
x = tx;
y = ty;
z = tz;
w = tw;
}
/**
* @brief 重载运算符
*
*/
Quaternion& operator*=(const double& s)
{
x *= s; y *= s; z *= s; w *= s;
return *this;
}
Quaternion& operator/=(const double& s)
{
//try catch的作用
assert(s != 0.0);
return *this *= 1.0 / s;
}
/**
* @brief 返回这个四元数和另一个四元数之间的点积。
*
* @param q
* @return double
*/
double dot( const Quaternion& q) const
{
return x * q.x + y * q.y + z * q.z + w * q.w;
}
/**
* @brief 返回这个四元数与自己的点积（平方）。
*
*/
double length2() const {return dot(*this);}
double tfSqrt(double x) const {return sqrt(x);}
double length()
{
//返回四元数和自身的点积的开方
return tfSqrt(length2());
}
Quaternion& normalize()
{
return *this /= length();
}
/**
* @brief 使用固定轴rpy设置四元数。
*
* @param roll
* @param pitch
* @param yaw
*/
void setRPY(const double& roll, const double& pitch, const double& yaw)
{
double halfYaw = (yaw) * (0.5);
double halfPitch = (pitch) * (0.5);
double halfRoll = (roll) * (0.5);
double cosYaw = cos(halfYaw);
double sinYaw = sin(halfYaw);
double cosPitch = cos(halfPitch);
double sinPitch = sin(halfPitch);
double cosRoll = cos(halfRoll);
double sinRoll = sin(halfRoll);
setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
}
};
上面的代码参照ros::tf中的代码，根据我们自己的通信机制进行重写。
注：
- tfScalar : 浮点数（说明上说可以在单精度和双精度之间转换），我这里直接用ros进行了替换
Matrix3x3类（旋转矩阵类，是一个3x3的矩阵，和四元数一样用来表示一个旋转，可以和四元数之间进行转化）
/**
* @brief 3x3的矩阵
*
*/
class Matrix3x3
{
public:
Vector3 m_el[3];
Matrix3x3() {}
inline Matrix3x3 (const Matrix3x3& other)
{
m_el[0] = other.m_el[0];
m_el[1] = other.m_el[1];
m_el[2] = other.m_el[2];
}
void setValue(double xx,double xy,double xz,double yx,double yy,double yz,double zx,double zy,double zz)
{
m_el[0].setValue(xx, xy, xz);
m_el[1].setValue(yx, yy, yz);
m_el[2].setValue(zx, zy, zz);
}
void getRotation(Quaternion& q) const
{
double trace = m_el[0].x + m_el[1].y + m_el[2].z;
double temp[4];
if (trace > (0.0))
{
double s = sqrt(trace + (1.0));
temp[3]=(s * (0.5));
s = (0.5) / s;
temp[0]=((m_el[2].y - m_el[1].z) * s);
temp[1]=((m_el[0].z - m_el[2].x) * s);
temp[2]=((m_el[1].x - m_el[0].y) * s);
}
else
{
int i = m_el[0].x < m_el[1].y ?
(m_el[1].y < m_el[2].z ? 2 : 1) :
(m_el[0].x < m_el[2].z ? 2 : 0);
int j = (i + 1) % 3;
int k = (i + 2) % 3;
double temp_mel[3][3];
for(int ti = 0; ti < 3; ti++) {
temp_mel[ti][0] = m_el[ti].x;
temp_mel[ti][1] = m_el[ti].y;
temp_mel[ti][2] = m_el[ti].z;
}
double s = sqrt(temp_mel[i][i] - temp_mel[j][j] - temp_mel[k][k] + (1.0));
temp[i] = s * (0.5);
s = (0.5) / s;
temp[3] = (temp_mel[k][j] - temp_mel[j][k]) * s;
temp[j] = (temp_mel[j][i] + temp_mel[i][j]) * s;
temp[k] = (temp_mel[k][i] + temp_mel[i][k]) * s;
}
q.setValue(temp[0],temp[1],temp[2],temp[3]);
}
void setRotation(const Quaternion& q)
{
double d = q.length2();
assert(d != 0.0);
double s = 2.0 / d;
double xs = q.x * s, ys = q.y * s, zs = q.z * s;
double wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
double xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
double yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
setValue(1.0 - (yy + zz), xy - wz, xz + wy,
xy + wz, 1.0 - (xx + zz), yz - wx,
xz - wy, yz + wx, 1.0 - (xx + yy));
}
/**
* @brief 由四元数来构造3x3矩阵
*
* @param q
*/
Matrix3x3(const Quaternion& q)
{
setRotation(q);
}
void getEulerYPR(double& yaw, double& pitch, double& roll, unsigned int solution_number = 1)
{
struct Euler
{
double yaw;
double pitch;
double roll;
};
Euler euler_out;
Euler euler_out2; //second solution
//get the pointer to the raw data
// Check that pitch is not at a singularity
// Check that pitch is not at a singularity
// if (tfFabs(m_el[2].x()) >= 1)
if (fabs(m_el[2].x) >= 1)
{
euler_out.yaw = 0;
euler_out2.yaw = 0;
// From difference of angles formula
if (m_el[2].x < 0) //gimbal locked down
{
double delta = atan2(m_el[0].y,m_el[0].z);
euler_out.pitch = TFSIMD_PI_ / (2.0);
euler_out2.pitch = TFSIMD_PI_ / (2.0);
euler_out.roll = delta;
euler_out2.roll = delta;
}
else // gimbal locked up
{
double delta = atan2(0-m_el[0].y,0-m_el[0].z);
euler_out.pitch = -TFSIMD_PI_ / 2.0;
euler_out2.pitch = -TFSIMD_PI_ / 2.0;
euler_out.roll = delta;
euler_out2.roll = delta;
}
}
else
{
euler_out.pitch = 0 - asin(m_el[2].x);
euler_out2.pitch = TFSIMD_PI_ - euler_out.pitch;
euler_out.roll = atan2(m_el[2].y/cos(euler_out.pitch),
m_el[2].z/cos(euler_out.pitch));
euler_out2.roll = atan2(m_el[2].y/cos(euler_out2.pitch),
m_el[2].z/cos(euler_out2.pitch));
euler_out.yaw = atan2(m_el[1].x/cos(euler_out.pitch),
m_el[0].x/cos(euler_out.pitch));
euler_out2.yaw = atan2(m_el[1].x/cos(euler_out2.pitch),
m_el[0].x/cos(euler_out2.pitch));
}
if (solution_number == 1)
{
yaw = euler_out.yaw;
pitch = euler_out.pitch;
roll = euler_out.roll;
}
else
{
yaw = euler_out2.yaw;
pitch = euler_out2.pitch;
roll = euler_out2.roll;
}
}
/**
* @brief 得到固定轴XYZ的滚动 俯仰 偏航矩阵
*
*/
void getRPY(double& roll, double& pitch, double& yaw, unsigned int solution_number = 1)
{
getEulerYPR(yaw, pitch, roll, solution_number);
}
};
函数：quaternionMsgToTF （用来将四元数类型转换为可以进行的通信的类型）
/**
* @brief 将自定义tf四元数类型转换为自定义四元数通信类型
*
* @param msg
* @param bt
*/
static inline void quaternionTFToMsg(const Quaternion& bt, geometry_msgs_Quaternion& msg)
{
if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE_)
{
std::cout<<"TF to MSG: Quaternion Not Properly Normalized";
Quaternion bt_temp = bt;
bt_temp.normalize();
msg.x = bt_temp.x; msg.y = bt_temp.y; msg.z = bt_temp.z; msg.w = bt_temp.w;
}
else
{
msg.x = bt.x; msg.y = bt.y; msg.z = bt.z; msg.w = bt.w;
}
}
3、移植image节点中的部分内容
image节点中有3个队列，原本存储的是从传感器（imu、雷达）的获取的数据以及里程计的数据。这些队列本身参与了算法，并且涉及到TF相关的内容，所以不方便直接修改。但是在重写了TF中的相关函数和类之后，就可以继续进行移植工作。
下面是3个队列，分别存储对应的数据。
我的工作主要是：
- 从订阅传感器发布话题的回调函数开始，先将接收的ros类型数据转换成自定义数据类型（这个问题是因为暂时没有处理原始数据的接收，只能暂时从ros数据包中接收数据，所以接收ros类型）。
- 每个回调函数的作用都是将接收到的数据存储到对应的队列中。这些数据会在其他函数中被使用，所以相应的要修改使用这些数据的地方
- 在函数中存在的和TF相关的代码也要重构成我重写的类型
下面是代码的部分展示：
接收原始数据
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
{
sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
std::lock_guard<std::mutex> lock1(imuLock);
//imuQueue.push_back(thisImu);
sensor_msgs_Imu temp_imu;
pub_change_sensor_msgs_Imu(thisImu, temp_imu);
imuQueue_struct.push_back(temp_imu);
}
具体移植（部分）
// tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
tf_struct::quaternionMsgToTF(endOdomMsg_struct.pose.pose.orientation, orientation_struct);
tf_struct::Matrix3x3(orientation_struct).getRPY(roll, pitch, yaw);
// Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);
Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg_struct.pose.pose.position.x, endOdomMsg_struct.pose.pose.position.y, endOdomMsg_struct.pose.pose.position.z, roll, pitch, yaw);
Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
float rollIncre, pitchIncre, yawIncre;
pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);
odomDeskewFlag = true;
//其他移植代码不具体展示
rosFunc.h（imu相关的数据类型转换）
template<typename T>
void imuRPY2rosRPY_struct(sensor_msgs_Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
{
double imuRoll, imuPitch, imuYaw;
// tf::Quaternion orientation;
// tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
// tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
tf_struct::Quaternion orientation;
tf_struct::quaternionMsgToTF(thisImuMsg->orientation, orientation);
tf_struct::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
*rosRoll = imuRoll;
*rosPitch = imuPitch;
*rosYaw = imuYaw;
}
template<typename T>
void imuAngular2rosAngular_struct(sensor_msgs_Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
{
*angular_x = thisImuMsg->angular_velocity.x;
*angular_y = thisImuMsg->angular_velocity.y;
*angular_z = thisImuMsg->angular_velocity.z;
}
技术难点
TF相关内容的学习和代码的编写
bug记录
bug总结
在数据传输过程中发现问题
- 然后发现是movefromROSMsg的问题
- 但是问题并不直接出在这个函数上
- 发现是队列中存储的数据有问题，导致转换了有了问题
- 然后发现queue中存储的变量没问题，数组除了问题
- 发现是指针的问题（野指针，push到vector中的数据类型中存在指针类型，这个指针指向的位置会在程序跳出该函数后失效），由于在原始的程序中，使用vector代替的指针，所以不存在上面的问题
现在还有个小问题，没过几个点云会有几百个data错误
-
错误的都是一段连续的
-
经过测试后发现，原因是：指针没有初始化，不知道为啥不会报错非法内存
其他
ros::tf API文档： http://docs.ros.org/en/noetic/api/tf/html/c++/namespacetf.html
robot_state_publisher话题在项目中出现了，但是没用到。到ros wiki上查阅后发现该话题的作用是用来维护机器人的不同部分（动态）之间的关系，所以本项目雀氏用不到，也就不需要进行相关的移植了。
339
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
