Ask Your Question
0

Computing a Motion Plan in an Action Server ExecuteCallback

asked 2019-01-09 15:28:39 -0500

jbeck28 gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-01-10 02:40:11 -0500

gvdhoorn gravatar image

updated 2019-01-10 02:46:54 -0500

I see the following in your code:

void executeCB(const optimax_runpath::runpathGoalConstPtr &goal) {
    [..]
    ros::spin();
    [..]
}

ros::spin() is a blocking call. Your callback will be blocked by this until ROS/your node shuts down.


Edit: and off-topic, but as a suggestion: try to use parameters more to make your code more flexible. Things like tool and user frames can be stored in .yaml files loaded with rosparam tags in your launch file. You could then initialise UF and TF in one ros::NodeHandle::getParam(..) call (docs). Same goes for your meshes.

That would probably simplify the code significantly (remove the need for sequences like if(robotName.compare("..") .. and the ladder code).


Edit2: another observation:

tableLoc = "package://../some_mesh.STL";

Does the suffix Loc mean that tableLoc is both a mesh as well as an (implicit) location of a part/obstacle? Are you relying on the mesh's origin for it to appear in the correct location?

That can work, but would seem to be brittle as well as reduce reusability of the mesh. An alternative approach is to make sure the mesh has a local origin that is inside/close to the geometry and then use a fixed joint (or similar) to place it at the correct location in your scene.

If Loc means something else, then please ignore this comment.

edit flag offensive delete link more

Comments

I attempted the same without that ros::spin(). I should have removed that as I knew that blocking calls in callbacks were a no-no. Should the asyncSpinner in the main loop allow my movegroup to work in the callback?

jbeck28 gravatar imagejbeck28 ( 2019-01-10 08:35:32 -0500 )edit

Also, thank you for the code improvement suggestions. I've been looking for simpler ways to initialize planning scenes and some geometry. I did have User Frames stored in a yaml file, but never went further with it for some reason.

jbeck28 gravatar imagejbeck28 ( 2019-01-10 08:36:48 -0500 )edit
1

Oops, the mistake was a much simpler one than expected. If anyone reads this in the future... AsyncSpinner in the main function suffices, in conjunction with ros::waitForShutdown()... and make sure your trajectory isn't empty...........

Thanks for your help gvdhoorn!

jbeck28 gravatar imagejbeck28 ( 2019-01-10 11:30:45 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-01-09 15:28:39 -0500

Seen: 68 times

Last updated: Jan 10