Issue with using planner on real drone

[xManjunathPatilx]

Hello,

I was trying to configure this planner for real-drone. I have VLSAM odom and pointcloud topics available.
The ego planner node is able to recognize the odom and cloud topic and also plans the trajectory/path around the obstacles(from real pointcloud data).
But the issue is that, the path is execute on its own without the real-drone even moving (drone is still at origin [0,0,0]), but the ego planner somehow increments the odometry and executes the original trajectory that was planned and reached the traget.

I tried to debug the issue, at line 572, /src/planner/plan_manage/src/ego_replan_fsm.cpp.

ego-planner-swarm/src/planner/plan_manage/src/ego_replan_fsm.cpp

Line 572 in 23a8d5a

Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur);

the variable Eigen::Vector3d pos, keep on incrementing on its own (in all 3 axis) according to the 3d path generated, and does not take the pos data of the actual real-drone.

Please help to resolve this issue. so that i can try it on real-drone.

How to reproduce the error?
step 1 : launch the below launch file (no simulator node is added)
step 2 : provide some odometry data on /drone_0_visual_slam/odom topic ( [0,0,0] is also sufficient)
step 3 : provide some point cloud on /drone_0_pcl_render_node/cloud (some random data is fine, so that the planner generates a 3d path around the object.
observation : the plan is executed on its own, and once completed, ego planner node is in wait_target state for next target goal
[ego_planner_node-1] [FSM]: state: WAIT_TARGET

EGO-PLANNER LAUNCH FILE USED, only one drone, no swarm

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import PythonExpression
from launch.conditions import IfCondition, UnlessCondition

def generate_launch_description():
    # 定义参数的 LaunchConfiguration
    obj_num = LaunchConfiguration('obj_num', default=10)
    drone_id = LaunchConfiguration('drone_id', default=0)
    
    map_size_x = LaunchConfiguration('map_size_x', default = 50.0)
    map_size_y = LaunchConfiguration('map_size_y', default = 25.0)
    map_size_z = LaunchConfiguration('map_size_z', default = 2.0)
    odom_topic = LaunchConfiguration('odom_topic', default = 'visual_slam/odom')
    # odom_topic = LaunchConfiguration('odom_topic', default = '/odometry')
    
    # 声明全局参数
    obj_num_cmd = DeclareLaunchArgument('obj_num', default_value=obj_num, description='Number of objects')
    drone_id_cmd = DeclareLaunchArgument('drone_id', default_value=drone_id, description='Drone ID')
    
    map_size_x_cmd = DeclareLaunchArgument('map_size_x', default_value=map_size_x, description='Map size along x')
    map_size_y_cmd = DeclareLaunchArgument('map_size_y', default_value=map_size_y, description='Map size along y')
    map_size_z_cmd = DeclareLaunchArgument('map_size_z', default_value=map_size_z, description='Map size along z')
    odom_topic_cmd = DeclareLaunchArgument('odom_topic', default_value=odom_topic, description='Odometry topic')

    # 地图属性以及是否使用动力学仿真
    use_mockamap = LaunchConfiguration('use_mockamap', default=False) # map_generator or mockamap 
    
    use_mockamap_cmd = DeclareLaunchArgument('use_mockamap', default_value=use_mockamap, description='Choose map type, map_generator or mockamap')
    
    use_dynamic = LaunchConfiguration('use_dynamic', default=False)  
    use_dynamic_cmd = DeclareLaunchArgument('use_dynamic', default_value=use_dynamic, description='Use Drone Simulation Considering Dynamics or Not')
    
    
    # Include advanced parameters
    advanced_param_include = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(
            get_package_share_directory('ego_planner'), 'launch', 'advanced_param.launch.py')),
        launch_arguments={
            'drone_id': drone_id,
            'map_size_x_': map_size_x,
            'map_size_y_': map_size_y,
            'map_size_z_': map_size_z,
            'odometry_topic': odom_topic,
            'obj_num_set': obj_num,
            
            'camera_pose_topic': 'pcl_render_node/camera_pose',
            'depth_topic': 'pcl_render_node/depth',
            'cloud_topic': 'pcl_render_node/cloud',
            
            'cx': str(321.04638671875),
            'cy': str(243.44969177246094),
            'fx': str(387.229248046875),
            'fy': str(387.229248046875),
            'max_vel': str(2.0),
            'max_acc': str(2.0),
            'planning_horizon': str(7.5),
            'use_distinctive_trajs': 'True',
            'flight_type': str(2),
            'point_num': str(1),
            'point0_x': str(15.0),
            'point0_y': str(0.0),
            'point0_z': str(1.0),
                        
            # 'point_num': str(4),
            # 'point0_x': str(15.0),
            # 'point0_y': str(0.0),
            # 'point0_z': str(1.0),
            
            # 'point1_x': str(-15.0),
            # 'point1_y': str(0.0),
            # 'point1_z': str(1.0),
            
            # 'point2_x': str(15.0),
            # 'point2_y': str(0.0),
            # 'point2_z': str(1.0),
            
            # 'point3_x': str(-15.0),
            # 'point3_y': str(0.0),
            # 'point3_z': str(1.0),
            
            # 'point4_x': str(15.0),
            # 'point4_y': str(0.0),
            # 'point4_z': str(1.0),
        }.items()
    )
    
    # Trajectory server node
    traj_server_node = Node(
        package='ego_planner',
        executable='traj_server',
        name=['drone_', drone_id, '_traj_server'],
        output='screen',
        remappings=[
            ('position_cmd', ['drone_', drone_id, '_planning/pos_cmd']),
            ('planning/bspline', ['drone_', drone_id, '_planning/bspline']),
            # ('odom_world', '/odometry')
        ],
        parameters=[
            {'traj_server/time_forward': 1.0}
        ]
    )
    
    ld = LaunchDescription()
        
    ld.add_action(map_size_x_cmd)
    ld.add_action(map_size_y_cmd)
    ld.add_action(map_size_z_cmd)
    ld.add_action(odom_topic_cmd)
    ld.add_action(obj_num_cmd)
    ld.add_action(drone_id_cmd)
    ld.add_action(use_dynamic_cmd)
    ld.add_action(use_mockamap_cmd)

    # 添加 Map Generator 节点
    # ld.add_action(map_generator_node)
    # ld.add_action(mockamap_node)
    ld.add_action(advanced_param_include)
    ld.add_action(traj_server_node)
    # ld.add_action(simulator_include)

    return ld

EGO PLANNER NODE OUTPUT

[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ego_planner_node-1]: process started with pid [70502]
[INFO] [traj_server-2]: process started with pid [70504]
[ego_planner_node-1] hit: 0.619039
[ego_planner_node-1] miss: -0.619039
[ego_planner_node-1] min log: -1.99243
[ego_planner_node-1] max: 2.19722
[ego_planner_node-1] thresh log: 1.38629
[ego_planner_node-1] [INFO] [1747587246.705055802] [drone_0_ego_planner_node]: Wait for 1 second.
[traj_server-2] [WARN] [1747587247.618898258] [drone_0_traj_server]: [Traj server]: ready.
[ego_planner_node-1] [FSM]: state: INIT
[ego_planner_node-1] no odom.
[ego_planner_node-1] wait for goal or trigger.
[ego_planner_node-1] [WARN] [1747587247.955647075] [drone_0_ego_planner_node]: Waiting for trigger from [n3ctrl] from RC
[ego_planner_node-1] [FSM]: state: INIT
[ego_planner_node-1] no odom.
[ego_planner_node-1] wait for goal or trigger.
[ego_planner_node-1] [FSM]: state: INIT
[ego_planner_node-1] no odom.
[ego_planner_node-1] wait for goal or trigger.
[ego_planner_node-1] [FSM]: state: INIT
[ego_planner_node-1] no odom.
[ego_planner_node-1] wait for goal or trigger.
[ego_planner_node-1] [FSM]: state: INIT
[ego_planner_node-1] no odom.
[ego_planner_node-1] wait for goal or trigger.
[ego_planner_node-1] no odom!
[ego_planner_node-1] [FSM]: from INIT to WAIT_TARGET
[ego_planner_node-1] [FSM]: from WAIT_TARGET to SEQUENTIAL_START
[ego_planner_node-1] [WARN] [1747587252.339552715] [distinctiveTrajs]: Can't find the new base points at the opposite within the threshold. i=0, j=0
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 0]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter=21,time(ms)=0.087,rebound.
[ego_planner_node-1] iter=147,time(ms)=0.59,rebound.
[ego_planner_node-1] iter(+1)=82,time(ms)=0.316,total_t(ms)=1.003,cost=0.173
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:0.0013,optimize:0.0013,refine:1.4e-06,avg_time=0.0013
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from SEQUENTIAL_START to EXEC_TRAJ
[ego_planner_node-1] [TRIG]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 1]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter=23,time(ms)=0.067,rebound.
[ego_planner_node-1] iter=9,time(ms)=0.021,rebound.
[ego_planner_node-1] iter=14,time(ms)=0.039,rebound.
[ego_planner_node-1] iter=5,time(ms)=0.016,rebound.
[ego_planner_node-1] iter(+1)=43,time(ms)=0.125,total_t(ms)=0.275,cost=0.328
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:0.00034,optimize:0.00034,refine:1.6e-06,avg_time=0.00084
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 2]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter(+1)=18,time(ms)=0.197,total_t(ms)=0.198,cost=0.026
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:0.00057,optimize:0.00057,refine:4.5e-06,avg_time=0.00075
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 3]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter(+1)=9,time(ms)=0.065,total_t(ms)=0.066,cost=0.017
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:0.00031,optimize:0.0003,refine:4.8e-06,avg_time=0.00064
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 4]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter(+1)=9,time(ms)=0.056,total_t(ms)=0.056,cost=0.011
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:0.00027,optimize:0.00027,refine:3.7e-06,avg_time=0.00057
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 5]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter(+1)=5,time(ms)=0.043,total_t(ms)=0.043,cost=0.008
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:0.00029,optimize:0.00028,refine:4.5e-06,avg_time=0.00052
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 6]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter(+1)=4,time(ms)=0.033,total_t(ms)=0.033,cost=0.003
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:0.00025,optimize:0.00024,refine:3.8e-06,avg_time=0.00048
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 7]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter(+1)=6,time(ms)=0.058,total_t(ms)=0.059,cost=0.019
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:0.00029,optimize:0.00029,refine:7.7e-06,avg_time=0.00046
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 8]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter(+1)=8,time(ms)=0.020,total_t(ms)=0.021,cost=0.023
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:0.00011,optimize:0.00011,refine:1.3e-06,avg_time=0.00042
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 9]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter(+1)=11,time(ms)=0.078,total_t(ms)=0.078,cost=0.036
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:0.00026,optimize:0.00025,refine:4.2e-06,avg_time=0.0004
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 10]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter(+1)=9,time(ms)=0.017,total_t(ms)=0.017,cost=0.024
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:7.2e-05,optimize:7.1e-05,refine:1.1e-06,avg_time=0.00037
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 11]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter(+1)=11,time(ms)=0.020,total_t(ms)=0.020,cost=0.049
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:6.7e-05,optimize:6.6e-05,refine:1.2e-06,avg_time=0.00035
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] 
[ego_planner_node-1] [drone 0 replan 12]==============================================
[ego_planner_node-1] multi-trajs=1
[ego_planner_node-1] iter(+1)=14,time(ms)=0.116,total_t(ms)=0.117,cost=0.022
[ego_planner_node-1] traj 1 success.
[ego_planner_node-1] plan_success=1
[ego_planner_node-1] total time:0.00033,optimize:0.00031,refine:2.5e-05,avg_time=0.00035
[ego_planner_node-1] refine_success=1
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [FSM]: state: EXEC_TRAJ
[ego_planner_node-1] [TRIG]: from EXEC_TRAJ to REPLAN_TRAJ
[ego_planner_node-1] [FSM]: from REPLAN_TRAJ to WAIT_TARGET
[ego_planner_node-1] [FSM]: state: WAIT_TARGET
[ego_planner_node-1] [FSM]: state: WAIT_TARGET
[ego_planner_node-1] [FSM]: state: WAIT_TARGET
[ego_planner_node-1] [FSM]: state: WAIT_TARGET
[ego_planner_node-1] [FSM]: state: WAIT_TARGET
[ego_planner_node-1] [FSM]: state: WAIT_TARGET
[ego_planner_node-1] [FSM]: state: WAIT_TARGET
[ego_planner_node-1] [FSM]: state: WAIT_TARGET
[ego_planner_node-1] [FSM]: state: WAIT_TARGET
[ego_planner_node-1] [FSM]: state: WAIT_TARGET
[ego_planner_node-1] [FSM]: state: WAIT_TARGET

ODOM TOPIC OUTPUT: drone at [0,0,0]. drone is not moved from its initial position

$ros2 topic echo /rtabmap/odom
header:
  stamp:
    sec: 1747417816
    nanosec: 655132080
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: -0.03433540463447571
      y: 0.017617087811231613
      z: 0.008380773477256298
    orientation:
      x: -0.002051307615177733
      y: 0.003184877680866669
      z: -0.014535661877499606
      w: 0.9998871752477921
  covariance:
  - 5.329395444074181e-05
  - 0.0
  - 0.0
  - .....