cartographer-project / cartographer

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.
Apache License 2.0
7.04k stars 2.24k forks source link

relocation by adding trajectory make memory(ram) usage bigger and bigger #1854

Open huangshuqing opened 2 years ago

huangshuqing commented 2 years ago

When I need to relocate with initial_pose, the cartographer use the way of adding a new trajectory. Such as :

Eigen::Quaterniond quaterniond_cur_to_traj0=cartographer::transform::RollPitchYaw(0,0,20);
geometry_msgs::PoseWithCovarianceStamped pose_msg;
pose_msg.header.stamp = ros::Time::now();
pose_msg.header.frame_id = "map";
pose_msg.pose.pose.position.x = 0;
pose_msg.pose.pose.position.y = 0;
pose_msg.pose.covariance[0] = 0.25;
pose_msg.pose.covariance[6 * 1 + 1] = 0.25;
pose_msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;
pose_msg.pose.pose.orientation.z = quaterniond_cur_to_traj0.z();
pose_msg.pose.pose.orientation.w = quaterniond_cur_to_traj0.w();

ros::NodeHandle nh;

ros::ServiceClient client_traj_finish = nh.serviceClient<cartographer_ros_msgs::FinishTrajectory>("finish_trajectory");
cartographer_ros_msgs::FinishTrajectory srv_traj_finish;
int traj_id = node->GetTrajectorySizeZYWN()-1;

LOG(INFO)<<"traj_id: "<<traj_id;
srv_traj_finish.request.trajectory_id = traj_id;
if (client_traj_finish.call(srv_traj_finish))
{
    ROS_INFO("Call finish_trajectory %d success!", traj_id);
}
else
{
    ROS_INFO("Failed to call finish_trajectory service!");
}

ros::ServiceClient client_traj_start = nh.serviceClient<cartographer_ros_msgs::StartTrajectory>("start_trajectory");
cartographer_ros_msgs::StartTrajectory srv_traj_start;
srv_traj_start.request.configuration_directory = "/home/cartographer_ros/configuration_files/";//.lua文件所在路径
srv_traj_start.request.configuration_basename = "zywn_backpack_3d_localization.lua";//lua文件
srv_traj_start.request.use_initial_pose = 1;
srv_traj_start.request.initial_pose = pose_msg.pose.pose;
srv_traj_start.request.relative_to_trajectory_id = 0;
if (client_traj_start.call(srv_traj_start))
{
    // ROS_INFO("Status ", srv_traj_finish.response.status)
    ROS_INFO("Call start_trajectory %d success!", traj_id);
    traj_id++;
}
else
{
    ROS_INFO("Failed to call start_trajectory service!");
}

However,it will result in trajectory size and memory(ram) usage bigger and bigger. If we keep repeating this operation, the system is getting stuck.

How to slove it ? or Why design like this?

thanks for answers!