void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
absl::MutexLock lock(&mutex_);
** _pose_pub = node_handle_.advertise<geometry_msgs::Pose2D>("pose_nav", 10);//100hz**
for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) {
// entry的数据类型为std::unordered_map<int,MapBuilderBridge::LocalTrajectoryData>
// entry.first 就是轨迹的id, entry.second 就是 LocalTrajectoryData
// 获取对应轨迹id的trajectory_data
const auto& trajectory_data = entry.second;
// 获取对应轨迹id的extrapolator
auto& extrapolator = extrapolators_.at(entry.first);
// We only publish a point cloud if it has changed. It is not needed at high
// frequency, and republishing it would be computationally wasteful.
// 如果当前状态的时间与extrapolator的lastposetime不等
if (trajectory_data.local_slam_data->time !=
extrapolator.GetLastPoseTime()) {
// 有订阅才发布scan_matched_point_cloud
if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) {
// TODO(gaschler): Consider using other message without time
// information.
carto::sensor::TimedPointCloud point_cloud;
point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
.returns.size());
// 获取local_slam_data的点云数据, 填入到point_cloud中
for (const cartographer::sensor::RangefinderPoint point :
trajectory_data.local_slam_data->range_data_in_local.returns) {
// 这里的虽然使用的是带时间戳的点云结构, 但是数据点的时间全是0.f
point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint(
point, 0.f /* time */));
}
// 先将点云转换成ROS的格式,再发布scan_matched_point_cloud点云
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
carto::common::ToUniversal(trajectory_data.local_slam_data->time),
node_options_.map_frame,
// 将雷达坐标系下的点云转换成地图坐标系下的点云
carto::sensor::TransformTimedPointCloud(
point_cloud, trajectory_data.local_to_map.cast<float>())));
} // end 发布scan_matched_point_cloud
// 将当前的pose加入到extrapolator中, 更新extrapolator的时间与状态
extrapolator.AddPose(trajectory_data.local_slam_data->time,
trajectory_data.local_slam_data->local_pose);
} // end if
geometry_msgs::TransformStamped stamped_transform;
// If we do not publish a new point cloud, we still allow time of the
// published poses to advance. If we already know a newer pose, we use its
// time instead. Since tf knows how to interpolate, providing newer
// information is better.
// 使用较新的时间戳
const ::cartographer::common::Time now = std::max(
FromRos(ros::Time::now()), extrapolator.GetLastExtrapolatedTime());
stamped_transform.header.stamp =
node_options_.use_pose_extrapolator
? ToRos(now)
: ToRos(trajectory_data.local_slam_data->time);
// Suppress publishing if we already published a transform at this time.
// Due to 2020-07 changes to geometry2, tf buffer will issue warnings for
// repeated transforms with the same timestamp.
if (last_published_tf_stamps_.count(entry.first) &&
last_published_tf_stamps_[entry.first] ==
stamped_transform.header.stamp)
continue;
// 保存当前的时间戳, 以防止对同一时间戳进行重复更新
last_published_tf_stamps_[entry.first] = stamped_transform.header.stamp;
const Rigid3d tracking_to_local_3d =
node_options_.use_pose_extrapolator
? extrapolator.ExtrapolatePose(now)
: trajectory_data.local_slam_data->local_pose;
// 获取当前位姿在local坐标系下的坐标
const Rigid3d tracking_to_local = [&] {
// 是否将变换投影到平面上
if (trajectory_data.trajectory_options.publish_frame_projected_to_2d) {
return carto::transform::Embed3D(
carto::transform::Project2D(tracking_to_local_3d));
}
return tracking_to_local_3d;
}();
// 求得当前位姿在map下的坐标
const Rigid3d tracking_to_map =
trajectory_data.local_to_map * tracking_to_local;
geometry_msgs::Pose2D pos_now;
// 根据lua配置文件发布tf
if (trajectory_data.published_to_tracking != nullptr) {
if (node_options_.publish_to_tf) {
// 如果需要cartographer提供odom坐标系
// 则发布 map_frame -> odom -> published_frame 的tf
if (trajectory_data.trajectory_options.provide_odom_frame) {
std::vector<geometry_msgs::TransformStamped> stamped_transforms;
// map_frame -> odom
stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id =
trajectory_data.trajectory_options.odom_frame;
// 将local坐标系作为odom坐标系
stamped_transform.transform =
ToGeometryMsgTransform(trajectory_data.local_to_map);
stamped_transforms.push_back(stamped_transform);
// odom -> published_frame
stamped_transform.header.frame_id =
trajectory_data.trajectory_options.odom_frame;
stamped_transform.child_frame_id =
trajectory_data.trajectory_options.published_frame;
// published_to_tracking 是局部坐标系下的位姿
stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_local * (*trajectory_data.published_to_tracking));
stamped_transforms.push_back(stamped_transform);
// 发布 map_frame -> odom -> published_frame 的tf
tf_broadcaster_.sendTransform(stamped_transforms);
}
// cartographer不需要提供odom坐标系,则发布 map_frame -> published_frame 的tf
else {
stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id =
trajectory_data.trajectory_options.published_frame;
stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_map * (*trajectory_data.published_to_tracking));
tf_broadcaster_.sendTransform(stamped_transform);
}
** geometry_msgs::Transform transform = ToGeometryMsgTransform(
tracking_to_map * (*trajectory_data.published_to_tracking));
pos_now.x = static_cast<double>(transform.translation.x);
pos_now.y = static_cast<double>(transform.translation.y);
// printf("x:%f y:%f\n", pos_now.x, pos_now.y);
pos_now.theta = tf2::getYaw(transform.rotation);
_pose_pub.publish(pos_now);**
}
**号部分为新加的，想要得到小车的位姿信息，头文件也加了，编译也通过了，但是一直没有pose_nav的话题
有时不将“调用函数名字＋各参数值，进入函数后各参数值，中间变量值，退出函数前准备返回的值，返回函数到调用处后函数名字＋各参数值＋返回值”这些信息写日志到文件中是无论如何也发现不了问题在哪里的，包括捕获各种异常、写日志到屏幕、单步或设断点或生成core或dmp文件、……这些方法都不行！ 写日志到文件参考下面：
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#ifdef _MSC_VER
#pragma warning(disable:4996)
#include <windows.h>
#include <io.h>
#else
#include <unistd.h>
#include <sys/time.h>
#include <pthread.h>
#define CRITICAL_SECTION pthread_mutex_t
#define _vsnprintf vsnprintf
#endif
//Log{
#define MAXLOGSIZE 20000000
#define MAXLINSIZE 16000
#include <time.h>
#include <sys/timeb.h>
#include <stdarg.h>
char logfilename1[]="MyLog1.log";
char logfilename2[]="MyLog2.log";
static char logstr[MAXLINSIZE+1];
char datestr[16];
char timestr[16];
char mss[4];
CRITICAL_SECTION cs_log;
FILE *flog;
#ifdef _MSC_VER
void Lock(CRITICAL_SECTION *l) {
EnterCriticalSection(l);
}
void Unlock(CRITICAL_SECTION *l) {
LeaveCriticalSection(l);
}
#else
void Lock(CRITICAL_SECTION *l) {
pthread_mutex_lock(l);
}
void Unlock(CRITICAL_SECTION *l) {
pthread_mutex_unlock(l);
}
#endif
void LogV(const char *pszFmt,va_list argp) {
struct tm *now;
struct timeb tb;
if (NULL==pszFmt||0==pszFmt[0]) return;
_vsnprintf(logstr,MAXLINSIZE,pszFmt,argp);
ftime(&tb);
now=localtime(&tb.time);
sprintf(datestr,"%04d-%02d-%02d",now->tm_year+1900,now->tm_mon+1,now->tm_mday);
sprintf(timestr,"%02d:%02d:%02d",now->tm_hour ,now->tm_min ,now->tm_sec );
sprintf(mss,"%03d",tb.millitm);
printf("%s %s.%s %s",datestr,timestr,mss,logstr);
flog=fopen(logfilename1,"a");
if (NULL!=flog) {
fprintf(flog,"%s %s.%s %s",datestr,timestr,mss,logstr);
if (ftell(flog)>MAXLOGSIZE) {
fclose(flog);
if (rename(logfilename1,logfilename2)) {
remove(logfilename2);
rename(logfilename1,logfilename2);
}
} else {
fclose(flog);
}
}
}
void Log(const char *pszFmt,...) {
va_list argp;
Lock(&cs_log);
va_start(argp,pszFmt);
LogV(pszFmt,argp);
va_end(argp);
Unlock(&cs_log);
}
//Log}
int main(int argc,char * argv[]) {
int i;
#ifdef _MSC_VER
InitializeCriticalSection(&cs_log);
#else
pthread_mutex_init(&cs_log,NULL);
#endif
for (i=0;i<10000;i++) {
Log("This is a Log %04d from FILE:%s LINE:%d\n",i, __FILE__, __LINE__);
}
#ifdef _MSC_VER
DeleteCriticalSection(&cs_log);
#else
pthread_mutex_destroy(&cs_log);
#endif
return 0;
}
//1-79行添加到你带main的.c或.cpp的那个文件的最前面
//82-86行添加到你的main函数开头
//90-94行添加到你的main函数结束前
//在要写LOG的地方仿照第88行的写法写LOG到文件MyLog1.log中