planner_gurobi.cpp

#include <math.h>
#include <cmath>
#include <vector>
#include <string>
#include <random>

#include <tf/tf.h>
#include <std_msgs/Bool.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <tf2_ros/transform_listener.h>

#include <robust_fast_navigation/utils.h>
#include <robust_fast_navigation/spline.h>
#include <robust_fast_navigation/corridor.h>
#include <robust_fast_navigation/planner_gurobi.h>

/**********************************************************************
  Constructor to read in ROS params, setup subscribers & publishers,
  ros timers, and initialize class fields.
***********************************************************************/
Planner::Planner(ros::NodeHandle &nh)
{

    // ROS Params
    nh.param("robust_planner/w_max", _max_w, 3.);
    nh.param("robust_planner/v_max", _max_vel, 1.0);
    nh.param("robust_planner/a_max", _max_acc, 1.2);
    nh.param("robust_planner/j_max", _max_jerk, 4.0);
    nh.param("robust_planner/traj_dt", _traj_dt, .1);
    nh.param("robust_planner/is_barn", _is_barn, false);
    nh.param("robust_planner/teleop", _is_teleop, false);
    nh.param("robust_planner/lookahead", _lookahead, .15);
    nh.param("robust_planner/max_deviation", _max_dev, 1.);
    nh.param("robust_planner/planner_frequency", _dt, .1);
    nh.param("robust_planner/plan_once", _plan_once, false);
    nh.param("robust_planner/use_minvo", _use_minvo, false);
    nh.param("robust_planner/plan_in_free", _plan_in_free, false);
    nh.param("robust_planner/simplify_jps", _simplify_jps, false);
    nh.param("robust_planner/failsafe_count", _failsafe_count, 2);
    nh.param("robust_planner/barn_goal_dist", _barn_goal_dist, 10.);
    nh.param("robust_planner/recovery_thresh", _recovery_thresh, .5);
    nh.param<std::string>("robust_planner/frame", _frame_str, "map");
    nh.param("robust_planner/max_dist_horizon", _max_dist_horizon, 4.);
    nh.param("robust_planner/enable_recovery", _enable_recovery, true);
    nh.param("robust_planner/recovery_samples", _recovery_samples, 10);
    nh.param("robust_planner/recovery_horizon", _recovery_horizon, 20);

    // Publishers
    trajVizPub =
        nh.advertise<visualization_msgs::Marker>("/MINCO_path", 0);
    wptVizPub =
        nh.advertise<visualization_msgs::Marker>("/MINCO_wpts", 0);
    trajPub =
        nh.advertise<trajectory_msgs::JointTrajectory>("/reference_trajectory", 0);
    trajPubNoReset =
        nh.advertise<trajectory_msgs::JointTrajectory>("/reference_trajectory_no_reset", 0);

    meshPub =
        nh.advertise<visualization_msgs::Marker>("/visualizer/mesh", 1000);
    edgePub =
        nh.advertise<visualization_msgs::Marker>("/visualizer/edge", 1000);

    helperMeshPub =
        nh.advertise<visualization_msgs::Marker>("/visualizer/mesh_helper", 1000);
    helperEdgePub =
        nh.advertise<visualization_msgs::Marker>("/visualizer/edge_helper", 1000);

    initPointPub =
        nh.advertise<geometry_msgs::PointStamped>("/initPoint", 0);

    goalPub =
        nh.advertise<geometry_msgs::PoseStamped>("/global_planner/goal", 0);

    paddedLaserPub =
        nh.advertise<visualization_msgs::Marker>("/paddedObs", 0);

    jpsPub =
        nh.advertise<nav_msgs::Path>("/jpsPath", 0);
    jpsPubFree =
        nh.advertise<nav_msgs::Path>("/jpsPathFree", 0);

    jpsPointsPub =
        nh.advertise<visualization_msgs::Marker>("/jpsPoints", 0);

    currPolyPub =
        nh.advertise<geometry_msgs::PoseArray>("/currPoly", 0);

    corridorPub =
        nh.advertise<geometry_msgs::PoseArray>("/polyCorridor", 0);

    recoveryPolyPub =
        nh.advertise<geometry_msgs::PoseArray>("/recoveryPoly", 0);

    intGoalPub =
        nh.advertise<geometry_msgs::PoseArray>("/intermediate_goal", 0);

    cmdVelPub =
        nh.advertise<geometry_msgs::Twist>("/cmd_vel", 0);

    initialPVAJPub =
        nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/initial_pvaj", 0);

    ctrlPointPub = 
        nh.advertise<geometry_msgs::PoseArray>("/ctrl_points", 0);

    // Services
    estop_client = nh.serviceClient<std_srvs::Empty>("/switch_mode");

    // Subscribers
    joySub = nh.subscribe("/joy", 1, &Planner::joycb, this);
    mapSub = nh.subscribe("/map", 1, &Planner::mapcb, this);
    goalSub = nh.subscribe("/planner_goal", 1, &Planner::goalcb, this);
    laserSub = nh.subscribe("/front/scan", 1, &Planner::lasercb, this);
    odomSub = nh.subscribe("/odometry/filtered", 1, &Planner::odomcb, this);
    occSub = nh.subscribe("/occ_points", 10, &Planner::occlusionPointscb, this);
    MPCHorizonSub = nh.subscribe("/mpc_horizon", 1, &Planner::mpcHorizoncb, this);
    clickedPointSub = nh.subscribe("/clicked_point", 1, &Planner::clickedPointcb, this);
    pathSub = nh.subscribe("/global_planner/planner/plan", 1, &Planner::globalPathcb, this);

    // Timers
    goalTimer = nh.createTimer(ros::Duration(_dt / 2.0), &Planner::goalLoop, this);
    controlTimer = nh.createTimer(ros::Duration(_dt), &Planner::controlLoop, this);
    publishTimer = nh.createTimer(ros::Duration(_dt * 2), &Planner::publishOccupied, this);

    _is_occ = false;
    _is_init = false;
    _planned = false;
    _is_goal_set = false;
    _map_received = false;
    _stop_planning = false;
    _primitive_started = false;
    _is_costmap_started = false;

    _robo_state = NOMINAL;
    // _robo_state = RECOVERY;

    _prev_jps_cost = -1;
    _curr_horizon = _max_dist_horizon;

    local_costmap = nullptr;
    global_costmap = nullptr;

    ROS_INFO("Initialized planner!");

    double limits[3] = {_max_vel, _max_acc, _max_jerk};
    sentTraj.points.clear();

    solver.setN(6);
    solver.createVars();
    solver.setDC(.05);
    solver.setBounds(limits);
    solver.setForceFinalConstraint(true);
    solver.setFactorInitialAndFinalAndIncrement(1, 10, 1.0);
    solver.setThreads(0);
    solver.setWMax(_max_w);
    solver.setVerbose(0);
    solver.setUseMinvo(_use_minvo);

    // ROS_INFO("starting threads now");
}

Planner::~Planner()
{

    if (global_costmap)
        delete global_costmap;

    if (local_costmap)
        delete local_costmap;
}

/**********************************************************************
  Function to start main ros loop for planning node. Also starts the
  costmap processes.

  TODOS: Add mutex's to class fields to allow for multi-threading
***********************************************************************/
void Planner::spin()
{
    // tf::TransformListener tfListener(ros::Duration(10));

    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);

    // ROS_INFO("starting local costmap");
    // local_costmap = new costmap_2d::Costmap2DROS("local_costmap", tfBuffer);
    // local_costmap->start();

    // allow time for transforms and things to be published
    // ros::Duration(3).sleep();

    ROS_INFO("initializing global costmap");
    global_costmap = new costmap_2d::Costmap2DROS("global_costmap", tfBuffer);
    ROS_INFO("starting global costmap");
    global_costmap->start();
    ROS_INFO("done!");

    _is_costmap_started = true;

    ros::AsyncSpinner spinner(1);
    spinner.start();

    ros::waitForShutdown();
}

void Planner::joycb(const sensor_msgs::Joy::ConstPtr &msg)
{
    if (msg->buttons[1])
    {
        ROS_ERROR("stopping planning");
        _stop_planning = true;
    }
}

/**********************************************************************
  Function callback which reads goal position from a clicked point in
  RVIZ. When called, this function will set the _is_goal_set field to
  true, which is required for planning to begin.

  Inputs:
    - PointStamped message
***********************************************************************/
void Planner::clickedPointcb(const geometry_msgs::PointStamped::ConstPtr &msg)
{
    goal = Eigen::VectorXd(2);
    goal(0) = msg->point.x;
    goal(1) = msg->point.y;
    _is_goal_set = true;
    _is_goal_reset = true;
}

/**********************************************************************
  Function to project a point into the costmap for planning purposes.

  Inputs:
    - Goal coordinates

  TODOS: Implement :)
***********************************************************************/
void Planner::projectIntoMap(const Eigen::Vector2d &goal)
{
}

/**********************************************************************
  Function callback which reads a published goal pose. As with the
  clicked point callback function, this will set the _is_goal_set
  field to true, which is required for planning to begin.

  Inputs:
    - PoseStamped message
***********************************************************************/
void Planner::goalcb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    goal = Eigen::VectorXd(2);
    goal(0) = msg->pose.position.x;
    goal(1) = msg->pose.position.y;

    _is_goal_set = true;
    ROS_INFO("goal received!");
}

void Planner::occlusionPointscb(const geometry_msgs::PoseArray::ConstPtr &msg)
{
    _occ_point = Eigen::MatrixXd(2, 3);

    if (msg->poses.size() > 0)
    {
        geometry_msgs::Pose p1 = msg->poses[0];
        geometry_msgs::Pose p2 = msg->poses[1];

        _occ_point.row(0) = Eigen::Vector3d(p1.position.x, p1.position.y, p1.position.z);
        _occ_point.row(1) = Eigen::Vector3d(p2.position.x, p2.position.y, p2.position.z);
        _is_occ = true;
    }
}

void Planner::mpcHorizoncb(const trajectory_msgs::JointTrajectory::ConstPtr &msg)
{
    mpcHorizon = *msg;
    _mpc_dt = mpcHorizon.points[1].time_from_start.toSec() - mpcHorizon.points[0].time_from_start.toSec();
}

/**********************************************************************
  Function which publishes the occupied cells of a global costmap.
***********************************************************************/
void Planner::publishOccupied(const ros::TimerEvent &)
{

    if (!_is_costmap_started)
        return;

    costmap_2d::Costmap2D *_map = global_costmap->getCostmap();
    std::vector<Eigen::Vector2f> padded = corridor::getOccupied(*_map);
    visualization_msgs::Marker paddedMsg;
    paddedMsg.header.frame_id = _frame_str;
    paddedMsg.header.stamp = ros::Time::now();
    paddedMsg.ns = "padded";
    paddedMsg.id = 420;
    paddedMsg.type = visualization_msgs::Marker::CUBE_LIST;
    paddedMsg.action = visualization_msgs::Marker::ADD;
    paddedMsg.scale.x = _map->getResolution();
    paddedMsg.scale.y = paddedMsg.scale.x;
    paddedMsg.scale.z = .1;
    paddedMsg.pose.orientation.w = 1;
    paddedMsg.color.r = 0.42;
    paddedMsg.color.g = 0.153;
    paddedMsg.color.b = 0.216;
    paddedMsg.color.a = .55;

    for (Eigen::Vector2f p : padded)
    {
        geometry_msgs::Point pMs;
        pMs.x = p[0];
        pMs.y = p[1];
        pMs.z = 0;
        paddedMsg.points.push_back(pMs);
    }

    paddedLaserPub.publish(paddedMsg);
}

/**********************************************************************
  Function callback which reads a published map. This map is different
  from the local and global costmap, since it doesn't contain an
  inflate layer around the obstacles.

  Inputs:
    - OccupancyGrid message

  TODOS: Decide if this map is needed or not for planning...
***********************************************************************/
void Planner::mapcb(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
    map = *msg;
    _map_received = true;
}

/**********************************************************************
  Function callback which reads in a published odometry message. To
  make arithmetic easier for planning purposes, it is converted in an
  (x,y,theta) format within an Eigen::Vector. The function also sets the
  _is_init flag to true, which is necessary for planning to begin.

  Inputs:
    - Odometry message

  TODOS: is vel field really necessary?
***********************************************************************/
void Planner::odomcb(const nav_msgs::Odometry::ConstPtr &msg)
{

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

    tf::Matrix3x3 m(q);
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);

    _odom = Eigen::VectorXd(3);
    _odom(0) = msg->pose.pose.position.x;
    _odom(1) = msg->pose.pose.position.y;
    _odom(2) = yaw;

    if (_is_barn && !_is_goal_set)
    {
        goal = Eigen::VectorXd(2);
        goal(0) = _barn_goal_dist * cos(yaw) + _odom(0);
        goal(1) = _barn_goal_dist * sin(yaw) + _odom(1);
        _is_goal_set = true;

        ROS_INFO("yaw is %.4f", yaw);
        ROS_INFO("goal is %.4f\t%.4f", goal(0), goal(1));
    }

    _is_init = true;
}

/**********************************************************************
  Function callback which reads in a laserscan message. Only runs when
  _is_init flag is set to true in order to save on computation. In
  order to be useable by polygon generation code, the scan is saved
  into a vector of Eigen::Vector2d.

  Inputs:
    - LaserScan message

***********************************************************************/
void Planner::lasercb(const sensor_msgs::LaserScan::ConstPtr &msg)
{

    if (!_is_init)
        return;

    _obs.clear();

    for (int i = 0; i < msg->ranges.size(); i++)
    {
        if (msg->ranges[i] > msg->range_max || msg->ranges[i] < msg->range_min)
            continue;

        double d = msg->ranges[i];

        double angle = msg->angle_min + i * msg->angle_increment + _odom(2);
        double x = d * cos(angle) + _odom(0);
        double y = d * sin(angle) + _odom(1);

        _obs.push_back(Eigen::Vector2f(x, y));
    }
}

/**********************************************************************
  Function callback which reads in a global path plan from an external
  node. This was used before JPS was implemented and will probably be
  removed in the near future if I don't find a need for it...

  Inputs:
    - Path message

  TODOS: Decide if this is a useful function now that JPS is
  implemented.
***********************************************************************/
void Planner::globalPathcb(const nav_msgs::Path::ConstPtr &msg)
{

    for (geometry_msgs::PoseStamped p : msg->poses)
        astarPath.push_back(Eigen::Vector2d(p.pose.position.x, p.pose.position.y));
}

/**********************************************************************
  Function which visualizes a given trajectory in RVIZ as a LINE_STRIP
  visualization_msg. Currently uses an external header library called
  tinycolormap to get the color associated with a normalized velocity.

  Inputs:
    - No inputs, works on the sentTraj field.

  TODOS:
    - Break out hard coded velocity normalization constant into a
      ros param?
    - Move this to utils.h?
***********************************************************************/
void Planner::visualizeTraj()
{

    if (sentTraj.points.size() > 0)
    {
        visualization_msgs::Marker msg;
        msg.header.frame_id = _frame_str;
        msg.header.stamp = ros::Time::now();
        msg.ns = 'planTraj';
        msg.id = 80;
        msg.action = visualization_msgs::Marker::ADD;
        msg.type = visualization_msgs::Marker::LINE_STRIP;

        msg.scale.x = .1;
        msg.pose.orientation.w = 1;

        for (trajectory_msgs::JointTrajectoryPoint p : sentTraj.points)
        {
            Eigen::Vector3d vel_vec(p.velocities[0],
                                    p.velocities[1],
                                    p.velocities[2]);
            double vel = vel_vec.norm();

            tinycolormap::Color color = tinycolormap::GetColor(vel / 1.0, tinycolormap::ColormapType::Plasma);

            std_msgs::ColorRGBA color_msg;
            color_msg.r = color.r();
            color_msg.g = color.g();
            color_msg.b = color.b();
            color_msg.a = 1.0;
            msg.colors.push_back(color_msg);

            geometry_msgs::Point point_msg;
            point_msg.x = p.positions[0];
            point_msg.y = p.positions[1];
            point_msg.z = p.positions[2];
            msg.points.push_back(point_msg);
        }

        trajVizPub.publish(msg);
    }
}

/**********************************************************************
  Function for the main control loop of the planner, calls the plan()
  method which takes care of overall planning logic.

  Inputs:
    - Just a timer event which controls the rate at which the control
      loop is run.
***********************************************************************/
void Planner::controlLoop(const ros::TimerEvent &)
{

    static int count = 0;

    if (!_is_init || !_is_goal_set || !_is_costmap_started)
        return;

    if ((_odom(1) - goal(1)) * (_odom(1) - goal(1)) + (_odom(0) - goal(0)) * (_odom(0) - goal(0)) < .2)
        return;

    if (_robo_state == RECOVERY)
    {
        return;
    }

    /*************************************
    ************* UPDATE MAP *************
    **************************************/

    // global_costmap->resetLayers();
    global_costmap->updateMap();

    /*************************************
    **************** PLAN ****************
    **************************************/

    if (_plan_once && _planned)
        return;

    const costmap_2d::Costmap2D &_map = *global_costmap->getCostmap();

    unsigned int map_x, map_y;
    _map.worldToMap(_odom(0), _odom(1), map_x, map_y);

    if (!plan())
    {
        count++;
        if (count >= _failsafe_count)
            _curr_horizon *= .9;
    }
    else
    {
        count = 0;
        _curr_horizon /= .9;
        if (_curr_horizon > _max_dist_horizon)
            _curr_horizon = _max_dist_horizon;
    }

    if (count >= _failsafe_count)
    {
        if (plan(true))
        {
            count = 0;
            _curr_horizon /= .9;
        }
        else
            _curr_horizon *= .9;
    }
}

/**********************************************************************
  This function is run on a timer controlled by the _dt ros parameter,
  and only runs if both the _is_init and is_goal_set flags are set to
  true. The overall process is:
    1.  Find JPS path from a point along previous trajectory to goal.
    1a. If first iteration, plan starting from robot position instead.
    2.  Expand intersecting polytopes around JPS path.
    3.  Send polytope and other physical constraints into optimizer.
    4.  Stitch resulting trajectory to previous one and publish to
        external trajectory tracker.

    Inputs:
      - is_failsafe flag which dictates if robot will stop for next
        planning phase. This is set true when planning failed
        several times in a row.
***********************************************************************/
bool Planner::plan(bool is_failsafe)
{

    if (is_failsafe)
    {
        // ROS_INFO("*******************************");
        // ROS_INFO("*** FAILSAFE MODE  ENGAGED ****");
        // ROS_INFO("*******************************");
    }

    costmap_2d::Costmap2D *_map = global_costmap->getCostmap();
    hPolys.clear();

    bool not_first = false;

    // ROS_INFO("Setting up initial and final conditions");
    Eigen::MatrixXd initialPVAJ(3, 4);
    Eigen::MatrixXd finalPVAJ(3, 4);

    finalPVAJ << Eigen::Vector3d(goal(0), goal(1), 0),
        Eigen::Vector3d::Zero(),
        Eigen::Vector3d::Zero(),
        Eigen::Vector3d::Zero();

    ROS_INFO("sent traj has size %lu", sentTraj.points.size());

    ros::Time a = ros::Time::now();

    double p_start_t = 0.;
    if (mpcHorizon.points.size() == 0 || _is_teleop || _is_goal_reset)
    {
        initialPVAJ << Eigen::Vector3d(_odom(0), _odom(1), 0),
            Eigen::Vector3d::Zero(),
            Eigen::Vector3d::Zero(),
            Eigen::Vector3d::Zero();
        
    }
    else
    {
        not_first = true;

        // double t = (a - start).toSec() + _lookahead;

        // ROS_INFO("t is %.4f", t);
        double t = _lookahead;

        int trajInd = std::min((int)(t / _mpc_dt), (int)mpcHorizon.points.size() - 1);

        trajectory_msgs::JointTrajectoryPoint p = mpcHorizon.points[trajInd];

        p_start_t = p.time_from_start.toSec();

        geometry_msgs::PointStamped poseMsg;
        poseMsg.header.frame_id = "map";
        poseMsg.header.stamp = ros::Time::now();
        poseMsg.point.x = p.positions[0];
        poseMsg.point.y = p.positions[1];
        poseMsg.point.z = p.positions[2];
        initPointPub.publish(poseMsg);

        // position
        initialPVAJ.col(0) = Eigen::Vector3d(p.positions[0],
                                             p.positions[1],
                                             p.positions[2]);

        // velocity
        initialPVAJ.col(1) = Eigen::Vector3d(p.velocities[0],
                                             p.velocities[1],
                                             p.velocities[2]);

        // acceleration
        initialPVAJ.col(2) = Eigen::Vector3d(p.accelerations[0],
                                             p.accelerations[1],
                                             p.accelerations[2]);

        // jerk (stored in effort)
        initialPVAJ.col(3) = Eigen::Vector3d(p.effort[0],
                                             p.effort[1],
                                             p.effort[2]);

        if (is_failsafe)
        {
            initialPVAJ.col(1) = Eigen::Vector3d::Zero();
            initialPVAJ.col(2) = Eigen::Vector3d::Zero();
            initialPVAJ.col(3) = Eigen::Vector3d::Zero();
        }
    }

    // publish initial pvaj
    trajectory_msgs::JointTrajectoryPoint p;
    p.positions = {initialPVAJ(0, 0), initialPVAJ(1, 0), initialPVAJ(2, 0)};
    p.velocities = {initialPVAJ(0, 1), initialPVAJ(1, 1), initialPVAJ(2, 1)};
    p.accelerations = {initialPVAJ(0, 2), initialPVAJ(1, 2), initialPVAJ(2, 2)};
    p.effort = {initialPVAJ(0, 3), initialPVAJ(1, 3), initialPVAJ(2, 3)};
    p.time_from_start = ros::Duration(p_start_t);
    initialPVAJPub.publish(p);

    /*************************************
    ************ PERFORM  JPS ************
    **************************************/

    // ROS_INFO("Running JPS");
    JPSPlan jps;
    unsigned int sX, sY, eX, eY;
    _map->worldToMap(_odom(0), _odom(1), sX, sY);
    // _map->worldToMap(initialPVAJ.col(0)[0], initialPVAJ.col(0)[1], sX, sY);
    _map->worldToMap(goal(0), goal(1), eX, eY);

    jps.set_start(sX, sY);
    jps.set_destination(eX, eY);
    jps.set_occ_value(costmap_2d::INSCRIBED_INFLATED_OBSTACLE);

    jps.set_map(_map->getCharMap(), _map->getSizeInCellsX(), _map->getSizeInCellsY(),
                _map->getOriginX(), _map->getOriginY(), _map->getResolution());

    double ax, ay;
    jps.mapToWorld(sX, sY, ax, ay);
    jps.JPS();

    // ROS_INFO("getting path");
    std::vector<Eigen::Vector2d> jpsPath = jps.getPath(_simplify_jps);
    // ROS_INFO("got path");

    trajectory_msgs::JointTrajectory empty_msg;
    nav_msgs::Path empty_path;

    if (jpsPath.size() == 0)
    {
        // ROS_ERROR("JPS failed to find path");
        return false;
    }

    // ROS_INFO("finished JPS");

    /*************************************
    ************* REFINE JPS *************
    **************************************/

    Eigen::Vector2d goalPoint;
    if (_plan_in_free)
    {

        nav_msgs::Path jpsMsgFree;
        jpsMsgFree.header.stamp = ros::Time::now();
        jpsMsgFree.header.frame_id = _frame_str;
        std::vector<Eigen::Vector2d> jpsFree = getJPSInFree(jpsPath, *_map);
        // ROS_INFO("jpsFree size: %lu", jpsFree.size());

        for (Eigen::Vector2d p : jpsFree)
        {
            geometry_msgs::PoseStamped pMsg;
            pMsg.header = jpsMsgFree.header;
            pMsg.pose.position.x = p(0);
            pMsg.pose.position.y = p(1);
            pMsg.pose.position.z = 0;
            pMsg.pose.orientation.w = 1;
            jpsMsgFree.poses.push_back(pMsg);
        }

        jpsPubFree.publish(jpsMsgFree);

        finalPVAJ << Eigen::Vector3d(jpsFree.back()[0], jpsFree.back()[1], 0),
            Eigen::Vector3d::Zero(),
            Eigen::Vector3d::Zero(),
            Eigen::Vector3d::Zero();

        if (jpsFree.size() == 0)
        {
            // ROS_ERROR("JPSFree has size 0");
            return false;
        }

        jpsPath = jpsFree;
    }
    else
    {
        std::vector<Eigen::Vector2d> newJPSPath;

        if (truncateJPS(
                jpsPath,
                newJPSPath,
                _curr_horizon))
        {

            jpsPath = newJPSPath;

            finalPVAJ << Eigen::Vector3d(jpsPath.back()[0], jpsPath.back()[1], 0),
                Eigen::Vector3d::Zero(),
                Eigen::Vector3d::Zero(),
                Eigen::Vector3d::Zero();

            // ROS_INFO_STREAM("JPS path intersected circle at " << goalPoint);
        }
        else
        {
            // ROS_WARN("JPS path did not intersect circle around robot");
        }
    }

    ROS_INFO_STREAM("initial pvaj is:\n"
                    << initialPVAJ);
    ROS_INFO_STREAM("final pvaj is: \n"
                    << finalPVAJ.col(0).transpose());

    /*************************************
    ******** PUBLISH JPS TO RVIZ *********
    **************************************/

    nav_msgs::Path jpsMsg;
    jpsMsg.header.stamp = ros::Time::now();
    jpsMsg.header.frame_id = _frame_str;

    for (Eigen::Vector2d p : jpsPath)
    {
        geometry_msgs::PoseStamped pMsg;
        pMsg.header = jpsMsg.header;
        pMsg.pose.position.x = p(0);
        pMsg.pose.position.y = p(1);
        pMsg.pose.position.z = 0;
        pMsg.pose.orientation.w = 1;
        jpsMsg.poses.push_back(pMsg);
    }

    jpsPub.publish(jpsMsg);

    /*************************************
    ********* GENERATE POLYTOPES *********
    **************************************/

    // ROS_INFO("creating corridor");
    // don't need to clear hPolys before calling because method will do it
    if (!corridor::createCorridorJPS(jpsPath, *_map, hPolys, initialPVAJ, finalPVAJ))
    {
        // ROS_ERROR("CORRIDOR GENERATION FAILED");
        return false;
    }

    bool is_in_corridor = false;

    // if adjacent polytopes don't overlap, don't plan
    for (int p = 0; p < hPolys.size() - 1; p++)
    {
        if (!geo_utils::overlap(hPolys[p], hPolys[p + 1]))
        {
            // ROS_ERROR("CORRIDOR IS NOT FULLY CONNECTED");
            return false;
        }

        if (!is_in_corridor)
            is_in_corridor = isInPoly(hPolys[p], Eigen::Vector2d(initialPVAJ(0, 0), initialPVAJ(1, 0)));
    }

    corridor::visualizePolytope(hPolys, meshPub, edgePub);

    // ROS_INFO("generated corridor of size %lu", hPolys.size());

    if (_stop_planning)
        return false;

    /*************************************
    ******** GENERATE  TRAJECTORY ********
    **************************************/

    state initialState;
    state finalState;

    initialState.setPos(initialPVAJ(0, 0), initialPVAJ(1, 0), initialPVAJ(2, 0));
    initialState.setVel(initialPVAJ(0, 1), initialPVAJ(1, 1), initialPVAJ(2, 1));
    initialState.setAccel(initialPVAJ(0, 2), initialPVAJ(1, 2), initialPVAJ(2, 2));
    initialState.setJerk(initialPVAJ(0, 3), initialPVAJ(1, 3), initialPVAJ(2, 3));

    finalState.setPos(finalPVAJ.col(0));
    finalState.setVel(finalPVAJ.col(1));
    finalState.setAccel(finalPVAJ.col(2));
    finalState.setJerk(finalPVAJ.col(3));

    // ROS_INFO("setting up");
    solver.setX0(initialState);
    solver.setXf(finalState);
    solver.setPolytopes(hPolys);

    if (_is_occ)
        solver.setOcc(_occ_point);

    // ROS_INFO("solving");
    // time trajectory generation
    ros::Time start_solve = ros::Time::now();
    if (!solver.genNewTraj())
    {
        ROS_ERROR("solver could not find trajectory");
        return false;
    }
    else
    {
        std::cout << termcolor::green << "solver found trajectory" << termcolor::reset << std::endl;
    }
    ROS_INFO("time to solve is %.4f", (ros::Time::now() - start_solve).toSec());

    solver.fillX();
    /*************************************
    ******** STITCH  TRAJECTORIES ********
    **************************************/

    // ROS_INFO("stitching trajectories");

    if (sentTraj.points.size() != 0)
    {

        double t1 = std::round((ros::Time::now() - a).toSec() * 10.) / 10.;
        double t2 = std::round(_lookahead * 10.) / 10.;

        // ROS_INFO("[%.2f] t1 is %.2f\tt2 is %.2f", (ros::Time::now()-start).toSec(),t1, t2);

        int startInd = std::min((int)(t1 / _mpc_dt), (int)mpcHorizon.points.size() - 1) + 1;
        int trajInd = std::min((int)(t2 / _mpc_dt), (int)mpcHorizon.points.size() - 1);

        // ROS_INFO("[%.2f] startInd is %d\ttrajInd is %d", (ros::Time::now()-start).toSec(),startInd, trajInd);

        // aTraj is the mpcHorizon up until the lookahead time
        trajectory_msgs::JointTrajectory aTraj, bTraj;
        bTraj = convertTrajToMsg(solver.X_temp_, _traj_dt, _frame_str);

        for (int i = startInd; i < trajInd; i++)
        {
            aTraj.points.push_back(mpcHorizon.points[i]);
            aTraj.points.back().time_from_start = ros::Duration((i - startInd) * _traj_dt);
        }

        double startTime = (trajInd - startInd) * _mpc_dt;

        for (int i = 0; i < bTraj.points.size(); i++)
        {
            aTraj.points.push_back(bTraj.points[i]);
            aTraj.points.back().time_from_start = ros::Duration(startTime + i * _traj_dt);
        }

        aTraj.header.frame_id = _frame_str;
        aTraj.header.stamp = ros::Time::now();
        sentTraj = aTraj;
        start = ros::Time::now();
        _is_goal_reset = false;
    }
    else
    {
        sentTraj = convertTrajToMsg(solver.X_temp_, _traj_dt, _frame_str);
        start = ros::Time::now();
    }

    if (!_is_teleop && _robo_state != RECOVERY)
        trajPub.publish(sentTraj);

    visualizeTraj();
    publishCPS();

    _planned = true;

    double totalT = (ros::Time::now() - a).toSec();
    // ROS_INFO("total time is %.4f", totalT);

    return true;
}

/**********************************************************************
  Function to publish current goal on a timer.

  Inputs:
    - Just a timer event which controls the rate at which the function
      is run.

  TODOS:
    - Decide if this function is necesary or not.
***********************************************************************/
void Planner::goalLoop(const ros::TimerEvent &)
{

    if (!_is_goal_set)
        return;

    geometry_msgs::PoseStamped msg;
    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = _frame_str;

    msg.pose.position.x = goal(0);
    msg.pose.position.y = goal(1);
    msg.pose.position.z = 0;
    msg.pose.orientation.w = 1;

    goalPub.publish(msg);
}

void Planner::publishCPS()
{
    // publish control points as a posearray
    geometry_msgs::PoseArray msg;
    msg.header.frame_id = _frame_str;
    msg.header.stamp = ros::Time::now();

    ROS_INFO("solver interval number is %d", solver.N_);

    for (int interval = 0; interval < solver.N_; interval++)
    {
        std::vector<GRBLinExpr> cp0 = solver.getCP0(interval);
        std::vector<GRBLinExpr> cp1 = solver.getCP1(interval);
        std::vector<GRBLinExpr> cp2 = solver.getCP2(interval);
        std::vector<GRBLinExpr> cp3 = solver.getCP3(interval);

        geometry_msgs::Pose msg_cp0;
        msg_cp0.position.x = cp0[0].getValue();
        msg_cp0.position.y = cp0[1].getValue();
        msg_cp0.position.z = cp0[2].getValue();
        msg.poses.push_back(msg_cp0);

        geometry_msgs::Pose msg_cp1;
        msg_cp1.position.x = cp1[0].getValue();
        msg_cp1.position.y = cp1[1].getValue();
        msg_cp1.position.z = cp1[2].getValue();
        msg.poses.push_back(msg_cp1);

        geometry_msgs::Pose msg_cp2;
        msg_cp2.position.x = cp2[0].getValue();
        msg_cp2.position.y = cp2[1].getValue();
        msg_cp2.position.z = cp2[2].getValue();
        msg.poses.push_back(msg_cp2);

        geometry_msgs::Pose msg_cp3;
        msg_cp3.position.x = cp3[0].getValue();
        msg_cp3.position.y = cp3[1].getValue();
        msg_cp3.position.z = cp3[2].getValue();
        msg.poses.push_back(msg_cp3);

    }

    ROS_INFO("msg poses size is %lu", msg.poses.size());
    ctrlPointPub.publish(msg);

}

void Planner::pubCurrPoly()
{

    geometry_msgs::PoseArray msg;

    // go backwards because we want the poly closest to end of corridor in
    // case of intersections
    for (int i = hPolys.size() - 1; i >= 0; i--)
    {
        Eigen::MatrixX4d poly = hPolys[i];

        if (isInPoly(poly, Eigen::Vector2d(_odom(0), _odom(1))))
        {
            for (int j = 0; j < poly.rows(); j++)
            {
                geometry_msgs::Pose p;
                p.orientation.x = poly.row(j)[0];
                p.orientation.y = poly.row(j)[1];
                p.orientation.z = poly.row(j)[2];
                p.orientation.w = poly.row(j)[3];
                msg.poses.push_back(p);
            }

            ROS_INFO("PUBLISHING POLY");
            currPolyPub.publish(msg);
            break;
        }
    }

    return;
}