Revision history [back]

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.

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.