Computing a Motion Plan in an Action Server ExecuteCallback

edit

I'm trying to implement motion control through an RVIZ panel, and I've decided to set up an action server between the gui (client) and another program which acts as the server. I have successfully gotten action messages to move between client and server, however the execute callback on the server is supposed to implement motion planning thru moveit, using the movegroupinterface. However, the terminal in which RVIZ is opened, simply hangs up on "Received request to compute Cartesian path". I'm nearly certain this is a problem with spinning and/or threads. Any help is appreciated. Below is the code for the action server, where the problems seem to arise.

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <optimax_runpath/runpathAction.h>
#include <rviz_loadtp_panel/toolpaths.h>
#include <optimax_utils/toolpath.h>
#include <optimax_utils/visualize.h>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometry_msgs/Pose.h>

class runpathAction {

protected: 
    ros::NodeHandle nh_; // nodehandle for server
    actionlib::SimpleActionServer<optimax_runpath::runpathAction> as_; // the action server itself
    std::string action_name_;
    // messages used to publish feedback and result.
    optimax_runpath::runpathFeedback feedback_;
    optimax_runpath::runpathResult   result_;

public:
    // constructor for runpathAction class
    runpathAction(std::string name) :
    as_(nh_, name, boost::bind(&runpathAction::executeCB, this, _1), false), action_name_(name)
    {
        as_.start();
        ROS_INFO("STARTED");
        group.reset(new moveit::planning_interface::MoveGroupInterface("manipulator"));
        UF.reserve(7);
        TF.reserve(7);
        nh_.getParam("robot_name",robotName); 
        if(robotName.compare("KB02") == 0){
            tableLoc = "package://deflectometry/meshes/Collide.dae";
            UF[0] = 610.396;
            UF[1] = 1.552;
            UF[2] = -229.515+330;
            UF[3] = .021;
            UF[4] = -.028;
            UF[5] = -89.972;
            UF[6] = 2;
            // FOLLOWING TOOL FRAME IS FOR THE MONITOR ON KB02
            TF[0] = 23.558;
            TF[1] = -.94;
            TF[2] = 515.264;
            TF[3] = 163.8;
            TF[4] = -2; 
            TF[5] = -94.3;
            TF[6] = 2;
        }
        else if(robotName.compare("KB04") == 0){
            tableLoc = "package://fanuc_lrmate200id_deflectometry_support/meshes/scene/kb04_Base_v3.dae";
            std::string panelLoc = "package://deflectometry/meshes/panels_only.dae";
            vis.attachMesh(Eigen::Affine3d::Identity(),"panels",panelLoc,rviz_visual_tools::colors::CLEAR);
            //partLoc = "package://optimax_utils/meshes/kb04/TestOptic.STL";//ogive section!!!
            partLoc = "package://optimax_utils/meshes/kb04/R200_Spinel_sphere.STL";//Spinel Sphere!!!

            /* // BIG WHEEL DROP SPINDLE, KB04
            TF[0] = -80.84;
            TF[1] = -.371;
            TF[2] = 217.852;
            TF[3] = 89.122;
            TF[4] = -.243; 
            TF[5] = 89.760;
            TF[6] = 2;
            */

            //nh.getParam("userframes/UF2",UF); // FOR OGIVE SECTION
            nh_.getParam("userframes/UF1",UF); // FOR SPINEL SPHERE
            std::cout << UF[0] << " , " << UF[1] << " , " << UF[2] << " , " << UF[3] << " , " << UF[4] << " , " << UF[5] << " , " << UF[6];

            UF[2]+=330;

            TF[0] = -80.7791;
            TF[1] = -.2378;
            TF[2] = 203.327;
            TF[3] = 89.122;
            TF[4] = -.243; 
            TF[5] = 89.760;
            TF[6] = 2;
            std::cout << "SELECTED TOOLFRAME" << TF[0] << std::endl;
            /* // user frame for spinel sphere
            UF[0] = 610.030;
            UF[1] = 1.327;
            //UF[2] = 575.462;
            UF[2] = -180.076+330;
            UF[3] = .169;
            UF[4] = -.244;
            UF[5] = -90;
            UF[6] = 2;
            */
        }
        else if(robotName.compare("KB05") == 0){
            //tableLoc = "package://abb_irb2400_deflectometry_support/meshes/scene/kb05_table.dae";
            tableLoc = "package://optimax_utils/meshes/KB05_PHIL.STL";
            partLoc  = "package ...