I want to write a program to subscribe two topics.And use these two topics in one callback function.

Here is my code

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "moveit_msgs/DisplayTrajectory.h" 
#include "trajectory_msgs/JointTrajectory.h"
#include "std_msgs/String.h"
#include "std_msgs/Float32MultiArray.h"
#include <iostream>
#include <math.h>

using namespace std;

class Controller{
    public:
        Controller();
    private:
        ros::NodeHandle n;
        ros::Subscriber curr_pose_sub;
        ros::Subscriber traj_sub;
        ros::Publisher local_pub;
        moveit_msgs::DisplayTrajectory traj_msg;
        geometry_msgs::PoseStamped curr_pose_msg,next_posi;

        void pose_callback(const geometry_msgs::PoseStamped::ConstPtr& pose){
        curr_pose_msg.pose = pose->pose;
        }

        void traj_callback(const moveit_msgs::DisplayTrajectory::ConstPtr& traj){
            if(traj->trajectory[0].multi_dof_joint_trajectory.points.size()>0){  
                for(int i=0;i < traj->trajectory[0].multi_dof_joint_trajectory.points.size(); ){
                    float curr_pose = sqrt(pow(curr_pose_msg.pose.position.x,2) + pow(curr_pose_msg.pose.position.y,2) + pow(curr_pose_msg.pose.position.z,2) )    ;
                    float traj_pose = sqrt(pow(traj->trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.x,2) + pow(traj->trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.y,2) + pow(traj->trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.z,2) )    ;

                    if(abs(curr_pose-traj_pose)<0.1 ){
                        i++; 
                    }
                    else {  
                        next_posi.pose.position.x =traj -> trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.x;
                        next_posi.pose.position.y =traj -> trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.y;
                        next_posi.pose.position.z =traj -> trajectory[0].multi_dof_joint_trajectory.points[i].transforms[0].translation.z;
                        next_posi.header =  traj-> trajectory[0].multi_dof_joint_trajectory.header;
                        print_curr_pose();
                        print_next_pose();
                        local_pub.publish(next_posi);
                    }
                }
            }

        }

        void print_curr_pose(){
        ROS_INFO("Seq: [%d]", curr_pose_msg.header.seq);
        ROS_INFO("Current Position-> x: [%f], y: [%f], z: [%f]", curr_pose_msg.pose.position.x,curr_pose_msg.pose.position.y, curr_pose_msg.pose.position.z);
        }

        void print_next_pose(){
        ROS_INFO("Seq: [%d]", next_posi.header.seq);
        ROS_INFO("Next Position-> x: [%f], y: [%f], z: [%f]", next_posi.pose.position.x,next_posi.pose.position.y, next_posi.pose.position.z);
        }

};

Controller::Controller(){
    curr_pose_sub = n.subscribe("/rtabmap/pose_cov", 1, &Controller::pose_callback, this);
    traj_sub = n.subscribe("/move_group/display_planned_path", 1, &Controller::traj_callback, this);
    local_pub = n.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 1, true);

}

int main(int argc, char **argv){
  ros::init(argc, argv, "controller");
  Controller controller;
  ros::spin();

  return 0;
}

But currposemsg is always the same in void trajcallback.I think maybe is in for loop so currposemsg will not update. Is there anyone can help me with how to update currpose_msg in for loop?