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
#include
#include
#include
#include
#include
#include
#include
#include
class runpathAction {
protected:
ros::NodeHandle nh_; // nodehandle for server
actionlib::SimpleActionServer 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://abb_irb2400_deflectometry_support/meshes/scene/AF_Window_CC.STL";
/*
// Nick - This is not the right UF for Phil
UF[0] = 885.305;
UF[1] = 3.26809;
UF[2] = 575.462;
//UF[2] = 275.462;
UF[3] = .708373;
UF[4] = -.000281113;
UF[5] = -.000320341;
UF[6] = -.705838;
*/
// Nick - This is whats on the ABB arm for PHIL
UF[0] = 885.305;
UF[1] = 3.26809;
UF[2] = 575.462;
UF[3] = .708373;
UF[4] = -.000281113;
UF[5] = -.000370341; //
UF[6] = -.705838;
// Nick added these
TF[0] = 81.834;
TF[1] = -2.3133;
TF[2] = 242.064;
TF[3] = .496201;
TF[4] = .50524;
TF[5] = -.495674;
TF[6] = -.502816;
}
toolpath.UF = UF;
toolpath.TF = TF;
}
// destructor for runpathAction class
~runpathAction(void){
}
void executeCB(const optimax_runpath::runpathGoalConstPtr &goal){
//std::cout << std::endl<< toolpath.UF[2] <<", tf: " << toolpath.TF[2]<< std::endl;
std::cout << goal->compute_path << "goal over here"<< std::endl;
if(goal->compute_path == 1){
toolpath.setup();
std::cout << "check check check" << std::endl;
std::cout << toolpath.fileloc << std::endl;
toolpath.loadpathQuat();
ma = vis.createMarkerArray(toolpath.VisualArray);//see line ~91
vis_pub.publish(ma);
std::cout <<"GOAL WAS ONE" << std::endl;
std::vector waypoints;
moveit_msgs::RobotTrajectory trajectory;
robot_trajectory::RobotTrajectory trajectory2(group->getRobotModel(),"manipulator");
const double jump_threshold = 0.0;
const double eef_step = .005;
double fraction = group->computeCartesianPath(waypoints, eef_step,
jump_threshold, trajectory, true);
ROS_INFO_NAMED("pathplan", "visualizing plan 4 (cartesian path) (%.2f%% acheived)"
,fraction*100.0);
feedback_.percent_complete = int(fraction*10000);
as_.publishFeedback(feedback_);
ros::spin();
ros::Duration sleepy(10);
sleepy.sleep();
}
}
// following is the subscriber callback for the topic "toolpaths", used exclusively to quickly store the toolpaths.
void chatterCallback(const rviz_loadtp_panel::toolpaths& msg)
{
toolpath_locations = msg.toolpaths;
std::cout<< toolpath_locations[0] < TF;
std::vector UF;
std::string robotName;
std::vector toolpath_locations; // stores the toolpaths sent over the topic "toolpaths"
optimax::toolpath toolpath;
optimax::visual_tools vis;
boost::scoped_ptr group;//{"manipulator"};
ros::Publisher vis_pub = nh_.advertise("visualization_marker_array", 1);
visualization_msgs::MarkerArray ma;
};
int main(int argc, char** argv){
ros::init(argc, argv, "runpath");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(0);
spinner.start();
runpathAction runpath("runpath");
std::vector UF(7);
ros::Subscriber sub = nh.subscribe("toolpaths", 100, &runpathAction::chatterCallback, &runpath);
ros::waitForShutdown();
return 0;
}
↧