完善后端代码,但还是有写问题

include/laser_odom.hpp

@@ -58,6 +58,7 @@ private:
 
   ros::NodeHandle nh;
   ros::Publisher odom_pub;      // 里程计发布
+  ros::Publisher path_pub;
   // ros::Publisher read_until_pub;  //
   ros::Publisher aligned_points_pub;
   ros::Publisher map_points_pub;

launch/start.launch

@@ -9,4 +9,6 @@
     <!-- 回放rosbag文件 -->
     <node pkg="rosbag" type="play" name="player" output="log" args="--clock /home/mj/hdl_slam/ros_yfm.bag"/>
 
+    <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher"  output="screen"/>
+
 </launch>

src/hdl_backend.cpp

@@ -29,7 +29,6 @@ int main(int argc, char* argv[]) {
 
 void hdl_backend::init() {
   map_frame_id = "map";
-  // map_frame_id = "odom";
   odom_frame_id = "odom";
   map_cloud_resolution = 0.05;   // 地图点云分辨率
   trans_odom2map.setIdentity();  // 设置为单位矩阵
@@ -55,11 +54,11 @@ void hdl_backend::init() {
 
   read_until_pub = nh.advertise<std_msgs::Header>("/hdl_graph_slam/read_until", 32);  // 这种类型的消息通常用于指示读取操作应该持续到何时
 
-  save_map_service_server = nh.advertiseService("/hdl_graph_slam/save_map", &hdl_backend::save_map_service, this);  // 保存地图
+  // save_map_service_server = nh.advertiseService("/hdl_graph_slam/save_map", &hdl_backend::save_map_service, this);  // 保存地图
 
   graph_updated = false;
 
-  double graph_update_interval = 1;       // 表示图优化更新的时间间隔，默认值为3.0秒    interval：间隔
+  double graph_update_interval = 1;      // 表示图优化更新的时间间隔，默认值为3.0秒    interval：间隔
   double map_cloud_update_interval = 4;  // 地图点云更新的时间间隔，默认值为10.0秒
 
   optimization_timer = nh.createWallTimer(ros::WallDuration(graph_update_interval), &hdl_backend::optimization_timer_callback, this);  // 创建一个定时器，定时器会在每个 graph_update_interval 秒后触发，然后调用 optimization_timer_callback 函数
@@ -75,13 +74,12 @@ void hdl_backend::init() {
  * @param event   // 是ROS提供的定时器时间类，包含信息有：计时器出发的时间戳、上一次出发的时间戳、计时器的周期信息等
  */
 void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event) {
-  LOG(INFO) << "optimization_timer_callback";
+  LOG(INFO) << "进入optimization_timer_callback函数";
 
   std::lock_guard<std::mutex> lock(main_thread_mutex);  // 创建了一个锁，用于保护对主线程的访问，确保线程安全
 
   // add keyframes and floor coeffs in the queues to the pose graph  将队列中的关键帧和楼层系数添加到姿势图中
   bool keyframe_updated = flush_keyframe_queue();  // 调用 flush_keyframe_queue 函数，将关键帧队列中的数据添加到位姿图中，并返回是否有关键帧被更新
-  // LOG(INFO) << keyframe_updated;
 
   if(!keyframe_updated) {  // 如果没有关键帧被更新，则执行大括号内的代码
     std_msgs::Header read_until;
@@ -93,9 +91,11 @@ void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event)
   }
   if(!keyframe_updated) {
     // 检查地板、GPS、IMU等队列，分别调用相应的 flush 函数（例如 flush_floor_queue、flush_gps_queue、flush_imu_queue），如果这些数据没有更新，则函数直接返回，不进行后续操作。
-    LOG(INFO) << "!keyframe_updated";
+    LOG(INFO) << "关键帧没有更新,退出optimization_timer_callback函数";
     return;
   }
+  
+
   // loop detection   闭环检测
   std::vector<hdl_graph_slam::Loop::Ptr> loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam);
 
@@ -106,8 +106,7 @@ void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event)
     auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);
     graph_slam->add_robust_kernel(edge, "Huber", 1.0);
   }
-  LOG(INFO) << "!111111111";
-
+  LOG(INFO) << "完成闭环检测";
 
   std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));  // 将新关键帧 new_keyframes 合并到已有的关键帧列表 keyframes 中
   new_keyframes.clear();                                                                 // 清空 new_keyframes
@@ -115,45 +114,46 @@ void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event)
   // move the first node anchor position to the current estimate of the first node pose   将第一节点锚点位置移动到第一节点姿态的当前估计值
   // so the first node moves freely while trying to stay around the origin                因此，第一个节点在试图停留在原点附近的同时可以自由移动
 
-  if(anchor_node && true) {  // launch文件中，fix_first_node_adaptive 设置为 true
+  if(anchor_node && true) {  // launch文件中，fix_first_node_adaptive 设置为 true   // 这个能不能设置为false
     Eigen::Isometry3d anchor_target = static_cast<g2o::VertexSE3*>(anchor_edge->vertices()[1])->estimate();
     anchor_node->setEstimate(anchor_target);
     // 如果启用了自适应固定第一帧的功能（参数 "fix_first_node_adaptive"），则将第一个关键帧的锚点位置更新为当前估计的位置，以允许它自由移动但仍然保持在原点附近。
   }
-  LOG(INFO) << "!2222222222";
   // optimize the pose graph
   int num_iterations = 512;              // launch文件中都是设置成512
   graph_slam->optimize(num_iterations);  // 使用 g2o 优化器对位姿图进行优化，优化的迭代次数由参数 "g2o_solver_num_iterations" 控制
-
+  LOG(INFO) << "完成g2o的图优化";
 
   std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot(keyframes.size());
   std::transform(keyframes.begin(), keyframes.end(), snapshot.begin(), [=](const hdl_graph_slam::KeyFrame::Ptr& k) { return std::make_shared<hdl_graph_slam::KeyFrameSnapshot>(k); });
 
-  keyframes_snapshot_mutex.lock();
+  // keyframes_snapshot_mutex.lock();
   keyframes_snapshot.swap(snapshot);
-  keyframes_snapshot_mutex.unlock();
+  // keyframes_snapshot_mutex.unlock();
   graph_updated = true;
-  LOG(INFO) << "graph_updated";
+  LOG(INFO) << "准备发布TF变换";
 
   // publish tf     发布位姿变换
   const auto& keyframe = keyframes.back();  // 获取最新的关键帧估计
   Eigen::Isometry3d trans = keyframe->node->estimate() * keyframe->odom.inverse();
-  trans_odom2map_mutex.lock();
+  // trans_odom2map_mutex.lock();
   trans_odom2map = trans.matrix().cast<float>();
-  trans_odom2map_mutex.unlock();
+  // trans_odom2map_mutex.unlock();
 
-  if(odom2map_pub.getNumSubscribers()) {
-    geometry_msgs::TransformStamped ts = hdl_graph_slam::matrix2transform(keyframe->stamp, trans.matrix().cast<float>(), map_frame_id, odom_frame_id);
-    odom2map_pub.publish(ts);  // 发布odom2map_pub话题中
-    LOG(INFO) << "pub odom2map_pub";
-  }
+  // if(odom2map_pub.getNumSubscribers()) {
+  geometry_msgs::TransformStamped ts = hdl_graph_slam::matrix2transform(keyframe->stamp, trans.matrix().cast<float>(), map_frame_id, odom_frame_id);
+  // ts.header.stamp = ros::Time::now();   // ?
+  // odom2map_pub.publish(ts);  // 发布odom2map_pub话题中/
+  LOG(INFO) << "odom2map TF变换发布完成";
+  // }
 
   if(markers_pub.getNumSubscribers()) {
     auto markers = create_marker_array(ros::Time::now());
     markers_pub.publish(markers);
-    LOG(INFO) << "pub markers_pub";
+    LOG(INFO) << "markers_pub 发布完成";
   }
 
+
 }
 
 /**
@@ -163,36 +163,57 @@ void hdl_backend::optimization_timer_callback(const ros::WallTimerEvent& event)
 void hdl_backend::map_points_publish_timer_callback(const ros::WallTimerEvent& event) {
   // LOG(INFO) << "map_points_publish_timer_callback";
 
-  if(!map_points_pub.getNumSubscribers() || !graph_updated) {
-    // LOG(INFO) << "cloud 222222";
+  // if(!map_points_pub.getNumSubscribers() || !graph_updated) {
+  //   // LOG(INFO) << "cloud 222222";
 
-    return;
-  }
-  // LOG(INFO) << "cloud 33333";
+  //   return;
+  // }
+  // // LOG(INFO) << "cloud 33333";
 
-  std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot;
+  // std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot;
 
-  // keyframes_snapshot_mutex.lock();
-  snapshot = keyframes_snapshot;
-  // keyframes_snapshot_mutex.unlock();
+  // // keyframes_snapshot_mutex.lock();
+  // snapshot = keyframes_snapshot;
+  // // keyframes_snapshot_mutex.unlock();
+
+  // auto cloud = map_cloud_generator->generate(snapshot, map_cloud_resolution);
+  // if(!cloud) {
+  //   return;
+  // }
+
+  // cloud->header.frame_id = map_frame_id;
+  // cloud->header.stamp = snapshot.back()->cloud->header.stamp;
+
+  // sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
+  // pcl::toROSMsg(*cloud, *cloud_msg);
+
+  // // cloud->header.frame_id = "odom";    //
+  // LOG(INFO) << cloud_msg->header.frame_id;
+
+  // map_points_pub.publish(*cloud_msg);
+  // cloud->header.frame_id = "map"; //
 
+  // 发布点云数据
+  LOG(INFO) << "准备发布点云数据";
+  std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot;
+  snapshot = keyframes_snapshot;
   auto cloud = map_cloud_generator->generate(snapshot, map_cloud_resolution);
+  LOG(INFO) << "获得点云数据";
   if(!cloud) {
     return;
   }
-
   cloud->header.frame_id = map_frame_id;
   cloud->header.stamp = snapshot.back()->cloud->header.stamp;
 
   sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
   pcl::toROSMsg(*cloud, *cloud_msg);
 
-  // cloud->header.frame_id = "odom";    // 
-  LOG(INFO) << cloud_msg->header.frame_id;
-
   map_points_pub.publish(*cloud_msg);
-  // cloud->header.frame_id = "map"; // 
-} 
+  LOG(INFO) << "点云数据发布完成";
+
+  LOG(INFO) << "map_points_publish_timer_callback 函数执行完毕";
+
+}
 
 /**
  * @brief 接收点云数据并放到 keyframe_queue 中
@@ -211,7 +232,7 @@ void hdl_backend::cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, con
   }
 
   // 更新关键帧判断
-  if(!keyframe_updater->update(odom)) {                      // 根据当前的里程计信息 odom 判断是否需要生成新的关键帧
+  if(!keyframe_updater->update(odom)) {  // 根据当前的里程计信息 odom 判断是否需要生成新的关键帧
     // LOG(INFO) << "!keyframe_updater->update(odom)";
     std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 这行代码的作用是确保线程安全，防止多个线程同时访问或修改 keyframe_queue 队列
     // std::lock_guard<std::mutex> 是 C++ 标准库中的一个类，用来简化互斥锁（std::mutex）的使用。它的作用是在其作用域内自动对给定的互斥量加锁，并在作用域结束时自动解锁

src/hdl_graph_slam/map2odom_publisher.py

@@ -34,8 +34,11 @@ def main():
 	rospy.init_node('map2odom_publisher')
 
 	# get some parameters to define what default frame_id's should be used while we wait for our first odom message
-	map_frame_id = rospy.get_param('~map_frame_id', 'map')
-	odom_frame_id = rospy.get_param('~odom_frame_id', 'odom')
+	# map_frame_id = rospy.get_param('~map_frame_id', 'map')
+	# odom_frame_id = rospy.get_param('~odom_frame_id', 'odom')
+	map_frame_id = 'map'
+	odom_frame_id = 'odom'
+
 
 	node = Map2OdomPublisher(odom_frame_id, map_frame_id)	
 

src/hdl_graph_slam/map_cloud_generator.cpp

@@ -3,6 +3,7 @@
 #include <hdl_graph_slam/map_cloud_generator.hpp>
 
 #include <pcl/octree/octree_search.h>
+#include <glog/logging.h>
 
 namespace hdl_graph_slam {
 
@@ -16,26 +17,51 @@ pcl::PointCloud<MapCloudGenerator::PointT>::Ptr MapCloudGenerator::generate(cons
     return nullptr;
   }
 
-  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
-  cloud->reserve(keyframes.front()->cloud->size() * keyframes.size());
+  // 问题定位到这里, 坐标转换没算对.
+  LOG(INFO) << "关键帧数量  keyframes.size() = " << keyframes.size();
+
+  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());    // 创建一个新的点云对象 cloud
+  cloud->reserve(keyframes.front()->cloud->size() * keyframes.size());  // 预留足够的内存空间，以容纳所有关键帧的点云数据
 
   for(const auto& keyframe : keyframes) {
-    Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();
+    Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();  // 每一帧关键帧相对第一帧的 pose 的齐次变换矩阵      这个齐次变换矩阵是对的
     std::cout << "pose: ===================:" << std::endl;
     std::cout << pose << std::endl;
+    int ii = 0;
     for(const auto& src_pt : keyframe->cloud->points) {
       PointT dst_pt;
-      dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
-      dst_pt.intensity = src_pt.intensity;
-      cloud->push_back(dst_pt);
+      // std::cout << ii++ << " ";
+      // dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap(); // ?
+
+
+
+
+      // 打印原始点坐标
+      // std::cout << "原始点坐标 (src_pt): " << src_pt.getVector4fMap().transpose() << std::endl;
+
+      // 进行变换
+
+      // dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
+      dst_pt.getVector4fMap() = src_pt.getVector4fMap();
+
+      // 打印变换后的点坐标
+      // std::cout << "变换后的点坐标 (dst_pt): " << dst_pt.getVector4fMap().transpose() << std::endl;
+
+
+
+
+
+      dst_pt.intensity = src_pt.intensity;  // 将源点的强度信息复制到目标点中。这通常用于保持激光点云的强度数据
+      cloud->push_back(dst_pt);             // 将变换后的点 dst_pt 添加到合并的点云 cloud 中
     }
   }
-
+  LOG(INFO)<<"gg";
   cloud->width = cloud->size();
   cloud->height = 1;
   cloud->is_dense = false;
 
   if(resolution <= 0.0) return cloud;  // To get unfiltered point cloud with intensity
+  // return cloud;
 
   pcl::octree::OctreePointCloud<PointT> octree(resolution);
   octree.setInputCloud(cloud);

src/laser_odom.cpp

@@ -30,6 +30,7 @@
 #include <hdl_graph_slam/ScanMatchingStatus.h>
 
 #include <laser_geometry/laser_geometry.h>
+#include <nav_msgs/Path.h>
 
 #include <tf2/LinearMath/Quaternion.h>
 
@@ -158,6 +159,13 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
     keyframe_stamp = stamp;
     prev_time = stamp;
     prev_trans.setIdentity();
+
+
+    sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
+    cloud_msg->header.frame_id = "map";
+    pcl::toROSMsg(*keyframe, *cloud_msg);
+
+    map_points_pub.publish(*cloud_msg);
   }
   //
   // Eigen::Matrix4f odom;
@@ -243,20 +251,20 @@ void Laser_Odom::publish_odometry(const ros::Time& stamp, const std::string& bas
 
   // 上面算出来的odom朝向有问题,需要顺时针转90度修正,代码如下:
   // 提取原始朝向
-  tf2::Quaternion original_orientation(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
+  // tf2::Quaternion original_orientation(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
 
-  // 定义顺时针旋转90度的四元数
-  tf2::Quaternion rotation_90(0, 0, -0.7071, 0.7071);
+  // // 定义顺时针旋转90度的四元数
+  // tf2::Quaternion rotation_90(0, 0, -0.7071, 0.7071);
 
-  // 计算旋转后的朝向
-  tf2::Quaternion new_orientation = rotation_90 * original_orientation;
-  new_orientation.normalize();  // 确保结果为单位四元数
+  // // 计算旋转后的朝向
+  // tf2::Quaternion new_orientation = rotation_90 * original_orientation;
+  // new_orientation.normalize();  // 确保结果为单位四元数
 
-  // 将旋转后的朝向赋值回原变量
-  odom.pose.pose.orientation.x = new_orientation.x();
-  odom.pose.pose.orientation.y = new_orientation.y();
-  odom.pose.pose.orientation.z = new_orientation.z();
-  odom.pose.pose.orientation.w = new_orientation.w();
+  // // 将旋转后的朝向赋值回原变量
+  // odom.pose.pose.orientation.x = new_orientation.x();
+  // odom.pose.pose.orientation.y = new_orientation.y();
+  // odom.pose.pose.orientation.z = new_orientation.z();
+  // odom.pose.pose.orientation.w = new_orientation.w();
 
   odom_pub.publish(odom);
 }