odom_state.cpp

 void odom_callback(const nav_msgs::Odometry::ConstPtr & msg){
            if (first_pass_) {
                first_pass_ = false;
                return ;
            }

            if (mpc_controller_created_) {
                MPCController::create_mpc_controller();
                return ;
            }

           
            tf2::Quaternion q(
                msg->pose.pose.orientation.x,
                msg->pose.pose.orientation.y,
                msg->pose.pose.orientation.z,
                msg->pose.pose.orientation.w);

            tf2::Matrix3x3 m(q);
            double roll, pitch, yaw;
            m.getRPY(roll, pitch, yaw);
            
            x(0) =  msg->twist.twist.linear.x;
            x(1) =  msg->twist.twist.linear.y;
            x(2) =  msg->twist.twist.linear.z;
            x(3) =  msg->twist.twist.linear.x;
            x(4) =  msg->twist.twist.linear.y;
            x(5) =  msg->twist.twist.linear.z;

            x(6) = msg->pose.pose.position.x;
            x(7) = msg->pose.pose.position.y;
            x(8) = msg->pose.pose.position.z;
            x(9) = roll;
            x(10) = pitch;
            x(11) = yaw;

        }