void minimumJerkHelper::visualizeTrajectory()
{
  if (visualize_trajectory_at_each_replanning_)
  {

    visualization_msgs::MarkerArray delete_msg;
    for (int i = 0; i < 10000; i++)
    {
      visualization_msgs::Marker marker_msg;
      marker_msg.header.frame_id = "world";
      marker_msg.action = visualization_msgs::Marker::DELETE;
      marker_msg.id = i;
      delete_msg.markers.push_back(marker_msg);
    }
    trajectory_visualization_pub_.publish(delete_msg);

    // send visualization markers
    visualization_msgs::MarkerArray msg;

    Eigen::Vector3d gravity_vector(0.0, 0.0, -9.81);

    visualization_msgs::Marker marker;
    int marker_id = 0;

    std::list < quad_common::QuadDesiredState > trajectory = trajectory_;
    quad_common::QuadDesiredState state_a, state_b;

    for (auto it = trajectory.begin(), end = std::prev(trajectory.end(), 1);
        it != end; ++it)
    {

      marker.header.frame_id = "world";
      marker.pose.orientation.w = 1.0;

      state_a = *it;
      state_b = *std::next(it, 1);

      marker.points.push_back(
          quad_common::vectorToPoint(
              quad_common::eigenToGeometry(state_a.position)));

      marker.points.push_back(
          quad_common::vectorToPoint(
              quad_common::eigenToGeometry(state_b.position)));

      marker.id = marker_id;
      marker.action = visualization_msgs::Marker::ADD;
      marker.type = visualization_msgs::Marker::LINE_STRIP;
      marker.scale.x = 0.015;
      marker.scale.y = 0.015;
      marker.scale.z = 0.015;

      marker.color.r = 0.0;
      marker.color.g = 1.0;
      marker.color.b = 0.0;

      marker.color.a = 1.0;
      msg.markers.push_back(marker);

      marker_id++;
    }

    trajectory_visualization_pub_.publish(msg);
  }

  else
  {
    ROS_WARN(
        "[%s] Visualization is disabled in the config, please enable it to see the trajectory in RVIZ",
        ros::this_node::getName().c_str());
  }

}