Issue With Using Planner On Real Drone

by ADMIN 39 views

Introduction

Using a planner on a real drone can be a challenging task, especially when it comes to integrating the planner with the drone's sensors and actuators. In this article, we will discuss an issue that arises when using a planner on a real drone, and provide a step-by-step guide on how to reproduce the error and resolve it.

Problem Description

The issue arises when the planner node is able to recognize the odom and cloud topic, but the path is executed on its own without the real-drone even moving. The ego planner somehow increments the odometry and executes the original trajectory that was planned and reached the target.

How to Reproduce the Error

To reproduce the error, follow these steps:

  1. Launch the provided launch file (no simulator node is added).
  2. Provide some odometry data on the /drone_0_visual_slam/odom topic ( [0,0,0] is also sufficient).
  3. Provide some point cloud on the /drone_0_pcl_render_node/cloud topic (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, the ego planner node is in the WAIT_TARGET state for the next target goal.

EGO-PLANNER LAUNCH FILE USED

The provided launch file is used to launch the ego-planner node. The launch file includes the following nodes:

  • ego_planner_node: The ego-planner node is responsible for planning the trajectory around the obstacles.
  • traj_server_node: The trajectory server node is responsible for sending the planned trajectory to the drone.

EGO PLANNER NODE OUTPUT

The ego-planner node output shows the following states:

  • INIT: The initial state of the ego-planner node.
  • WAIT_TARGET: The state of the ego-planner node when waiting for the next target goal.
  • EXEC_TRAJ: The state of the ego-planner node when executing the planned trajectory.
  • REPLAN_TRAJ: The state of the ego-planner node when replanning the trajectory.

ODOM TOPIC OUTPUT

The odom topic output shows the drone at [0,0,0]. The drone is not moved from its initial position.

Debugging the Issue

To debug the issue, we need to identify the source of the problem. The issue arises when the planner node is able to recognize the odom and cloud topic, but the path is executed on its own without the real-drone even moving. The ego planner somehow increments the odometry and executes the original trajectory that was planned and reached the target.

To resolve the issue, we need to modify the ego-planner node to take into account the actual position of the drone. We can do this by modifying the ego_replan_fsm.cpp file to use the actual position of the drone instead of the simulated position.

Modifying the Ego-Planner Node

To modify the ego-planner node, we need to make the following changes:

  1. Modify the ego_replan_fsm.cpp file to use the actual position of the drone instead of the simulated position.
  2. Modify the traj_server_node to send the planned trajectory to the drone based on the actual position of the drone.

Conclusion

In this article, we discussed an issue that arises when using a planner on a real drone. We provided a step-by-step guide on how to reproduce the error and resolve it. We also modified the ego-planner node to take into account the actual position of the drone. By following these steps, we can resolve the issue and use the planner on a real drone.

Code Modifications

The following code modifications are required to resolve the issue:

// ego_replan_fsm.cpp
// ...

// Modify the `pos` variable to use the actual position of the drone
Eigen::Vector3d pos = getActualPosition();

// ...

// Modify the `traj_server_node` to send the planned trajectory to the drone based on the actual position of the drone
// ...

// ...

Launch File Modifications

The following launch file modifications are required to resolve the issue:

# launch file
# ...

# Add the following line to the launch file to modify the `ego_planner_node` to use the actual position of the drone
ego_planner_node = Node(
    package='ego_planner',
    executable='ego_planner_node',
    name=['drone_', drone_id, '_ego_planner_node'],
    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}
    ],
    args=['--actual_position']
)

# ...

Note

Q: What is the issue with using a planner on a real drone?

A: The issue arises when the planner node is able to recognize the odom and cloud topic, but the path is executed on its own without the real-drone even moving. The ego planner somehow increments the odometry and executes the original trajectory that was planned and reached the target.

Q: How to reproduce the error?

A: To reproduce the error, follow these steps:

  1. Launch the provided launch file (no simulator node is added).
  2. Provide some odometry data on the /drone_0_visual_slam/odom topic ( [0,0,0] is also sufficient).
  3. Provide some point cloud on the /drone_0_pcl_render_node/cloud topic (some random data is fine, so that the planner generates a 3D path around the object).

Q: What is the observation when the plan is executed on its own?

A: The plan is executed on its own, and once completed, the ego planner node is in the WAIT_TARGET state for the next target goal.

Q: What is the odom topic output when the drone is not moved from its initial position?

A: The odom topic output shows the drone at [0,0,0]. The drone is not moved from its initial position.

Q: How to modify the ego-planner node to take into account the actual position of the drone?

A: To modify the ego-planner node, we need to make the following changes:

  1. Modify the ego_replan_fsm.cpp file to use the actual position of the drone instead of the simulated position.
  2. Modify the traj_server_node to send the planned trajectory to the drone based on the actual position of the drone.

Q: What are the code modifications required to resolve the issue?

A: The following code modifications are required to resolve the issue:

// ego_replan_fsm.cpp
// ...

// Modify the `pos` variable to use the actual position of the drone
Eigen::Vector3d pos = getActualPosition();

// ...

// Modify the `traj_server_node` to send the planned trajectory to the drone based on the actual position of the drone
// ...

// ...

Q: What are the launch file modifications required to resolve the issue?

A: The following launch file modifications are required to resolve the issue:

# launch file
# ...

# Add the following line to the launch file to modify the `ego_planner_node` to use the actual position of the drone
ego_planner_node = Node(
    package='ego_planner',
    executable='ego_planner_node',
    name=['drone_', drone_id, '_ego_planner_node'],
    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}
    ],
    args=['--actual_position']
)

# ...

Q: What is the note about the code modifications and launch file modifications?

A: The above code modifications and launch file modifications are required to resolve the issue. However, the actual code modifications and launch file modifications may vary depending on the specific implementation of the ego-planner node and the launch file.