node hanging at at MoveGroupInterface

[pathare535]

I've been struggling through implementing moveit on the iiwa and after searching and trying to follow the example launch files I have the robot launching and at least reading joint states as the Rviz model snaps to the real position at start up. The issue I'm having now is that my main node to command the robot seems to be hanging up at "moveit::planning_interface::MoveGroupInterface move_group("manipulator");" the full node is below.

#include <actionlib/client/terminal_state.h>
#include <perception_core/perceptionAction.h>
#include <perception_core/LocalizePart.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <tf/tf.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <iiwa_ros.h>

using namespace moveit;
int main(int argc, char **argv)
{
  sleep(2);
  ros::init(argc, argv, "perception_core");
  ros::NodeHandle nh;
  ros::NodeHandle priv_nh("~");

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

  iiwa_ros::iiwaRos my_iiwa;
  my_iiwa.init();

  std::string movegroup_name, ee_link;
  geometry_msgs::PoseStamped command_cartesian_position;

  // Dynamic parameters. Last arg is the default value. You can assign these from a launch file.
//  nh.param<std::string>("move_group", movegroup_name, "manipulator");
//  nh.param<std::string>("ee_link", ee_link, "ee_link");
  std::string world_frame;
  priv_nh.param<std::string>("world_frame", world_frame, "world");

 // ros::ServiceClient localize_part_client = nh.serviceClient<perception_core::LocalizePart>("localize_part");

  ROS_INFO("sending joint pose.");

//  perception_core::LocalizePart srv;

//    sleep(3);
//    srv.request.RefFrame = world_frame;
//    if(localize_part_client.call(srv))
//    {
//        ROS_INFO("service should be locked up");
//    }
//    ROS_INFO("Service started, sending goal.");

//     ROS_INFO_STREAM("part localized: " << srv.response);

//     geometry_msgs::Pose move_target = srv.response.pose;

       geometry_msgs::Pose move_target;
       move_target.position.x = 0.036;
       move_target.position.y = 0.276;
       move_target.position.z = 1.084;
       move_target.orientation.x = 0;
       move_target.orientation.y = 0;
       move_target.orientation.z = 0;
       move_target.orientation.w = 1;
       ROS_WARN("target created");

tf2::Quaternion q_orig, q_rot, q_new;

tf2::convert(move_target.orientation, q_orig);

double r=0, p=1.6, y=1.6;
q_rot.setRPY(r, p, y);

q_new = q_rot * q_orig;
q_new.normalize();

tf2::convert(q_new, move_target.orientation);

    ROS_INFO("quaternion rotated");
    // Create MoveGroup

    moveit::planning_interface::MoveGroupInterface move_group("manipulator");
    moveit::planning_interface::MoveGroupInterface::Plan myplan;

    ROS_INFO("move group created");
    // Configure planner
    move_group.setPlanningTime(0.5);
    move_group.setPlannerId("[RRTConnectkConfigDefault]");
    move_group.setEndEffectorLink(ee_link);

    ROS_INFO("planner configured");

    move_group.setStartStateToCurrentState();

    ROS_INFO("State set to current state");

    //plan for robot to move to part
    move_group.setPoseReferenceFrame(world_frame);
    move_group.setPoseTarget(move_target);
    move_group.plan(myplan);
    ROS_WARN("sending path plan");
      move_group.execute(myplan);

 ros::waitForShutdown();
  return 0;
}

the position I'm hard coding should be valid as I used the perception nodes for this project to find an object in the workspace and pass its position to the robot control node. I just hard coded it now so I don't have to launch the cameras.

any suggestions why its hanging up?

Thanks

[SalvoVirga]

The move group "manipulator" by default lives under the "iiwa" namespace (/iiwa/manipulator). If you look for "manipulator", it will search in the root namespace and fine nothing. You node should also live under the "iiwa" namespace

[SalvoVirga]

Does this solve your problem? In case please close the issue or let me know.

[pathare535]

Yes, sorry I got pulled to another project. Your comments were the solution to my problem. I will close this issue.

Thanks

[Oscar-B-Liang]

Same issue here, the whole code just got stuck at the step "moveit::planning_interface::MoveGroupInterface move_group("manipulator");" and seems this step never finishes. Changing the group name "manipulator" to "/iiwa/manipulation" simply throws exception saying the group name cannot be found.

Obviously the code can find the move group "manipulator" from its name, but it just never finish initializing, any suggestions why this happens?