Planning scene [closed]

edit

Hi everyone, I'm not sure, If I'm doing the correct using the planning_environtment. Base on WorldInterface class in pyton, and other examples in Internet, I did my World Interface class (after the text).

What I really want is create a world (with some objects) and move the arm with ompl a and interactuate with the objects. For this I do (schematically):

Do I use the planning_enviorenment wrong? I cant obtain a good plan.

My problem is: when I request a plan, I obtain a code Error: -1.

Regards.

World_interface world;
  ...
  arm_navigation_msgs::CollisionObject pieza1_object;
  pieza1_object.id = "pieza1_pole";
  pieza1_object.header.frame_id = "/odom_combined";
  ...
  world.add_object(pieza1_object);
  world.add_object(arrayObj[1]);
  world.add_object(arrayObj[2]);
  ...
    plan_request.motion_plan_request.goal_constraints.position_constraints[0].position.x = arrayObj[i].poses[0].position.x;
   plan_request.motion_plan_request.goal_constraints.position_constraints[0].position.y = arrayObj[i].poses[0].position.y;
  ...
  service_client.call(plan_request,plan_response);
  if(plan_response.error_code.val != plan_response.error_code.SUCCESS) {
  ...




class World_interface {

    private:
    //Atributes:
    ros::NodeHandle nh;

    ros::Publisher collision_object_pub;
    ros::Publisher attached_object_pub;
    ros::ServiceClient get_planning_scene;  //Returns the current state of the world.
    ros::ServiceClient set_planning_scene_diff;


    std::string world_frame; //(string): The frame in which objects are added to the collision map and also the frame in
    //which planning is done.  Usually this is /map, but if not using a map it will be /odom_combined.  It is
    //found by looking at the parent of the multi DOF joint in the robot state.

    std::string robot_frame; //(string): The root frame of the robot's link tree.  This has the same orientation as the
    //world frame but moves with the robot.  Usually it is /base_footprint.  It is found by looking at the child
    //of the multi DOF joint in the robot state.

    std::string hands; // (dictionary): Hand descriptions for the robot indexed by 'left_arm' and 'right_arm'.  For more
    //information about hand descriptions, see hand_description.py

    //Metodos:
    void publish(arm_navigation_msgs::AttachedCollisionObject msg_obj, ros::Publisher publisher) {

        arm_navigation_msgs::SetPlanningSceneDiff::Request set_planning_scene_diff_req;
        arm_navigation_msgs::SetPlanningSceneDiff::Response set_planning_scene_diff_res;

        ros::Duration(0.5).sleep();
        if(publisher == attached_object_pub) {
            publisher.publish(msg_obj);
        }
        else {
            publisher.publish(msg_obj.object);
        }

        ros::Duration(0.5).sleep();

        //Actualiza el environtment
        if(!set_planning_scene_diff.call(set_planning_scene_diff_req, set_planning_scene_diff_res)) {
            ROS_ERROR("Can't set planning scene");
        }
    }


public:
     World_interface() {

         //Servicios:
         while(!ros::service::waitForService(GET_PLANNING_SCENE_NAME, ros::Duration(1.0)))
             ROS_WARN("waiting for %s",GET_PLANNING_SCENE_NAME.c_str());

         while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0)))
             ROS_WARN("waiting for %s",SET_PLANNING_SCENE_DIFF_NAME.c_str());

         get_planning_scene = nh.serviceClient<arm_navigation_msgs::GetPlanningScene>(GET_PLANNING_SCENE_NAME);
         set_planning_scene_diff = nh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);

         //Publicadores:
         collision_object_pub = nh.advertise<arm_navigation_msgs::CollisionObject>("/collision_object", 10);
         attached_object_pub = nh.advertise<arm_navigation_msgs::AttachedCollisionObject>("/attached_collision_object", 10);

         world_frame = "odom_combined";
         robot_frame = "/base_footprint";
         hands = "right_arm";

         ROS_INFO("World interface initialized");
     }

     ~World_interface() {
         //ros::shutdown();
     }

     void reset_collision_objects() {
         //Removes all collision objects (but not attached objects!) from the collision map.
         arm_navigation_msgs::AttachedCollisionObject obj;
         obj.object = get_reset_object();

         this->publish(obj, collision_object_pub);

         ROS_INFO("Collision object reset");
     }

     void reset_attached_objects() {
         //Removes all collision objects that are currently attached to the robot.
         arm_navigation_msgs::AttachedCollisionObject obj;

         obj.link_name = "all";
         obj.object.header.frame_id = world_frame; //Frame referencia.
         obj.object.header.stamp = ros::Time::now();
         obj.object = get_reset_object();

         this->publish(obj, attached_object_pub);

         ROS_INFO("Attached object reset");
     }

     arm_navigation_msgs ...