Robotics StackExchange | Archived questions

Planning scene

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::CollisionObject get_reset_object() {
         arm_navigation_msgs::CollisionObject  obj;
         obj.header.frame_id = world_frame; //Frame referencia.
         obj.header.stamp = ros::Time::now();
         obj.id = "all";
         obj.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
         return obj;
     }

     void reset_all_objects() {
         //Removes all collision objects and all attached collision objects.
         reset_collision_objects();
         reset_attached_objects();
     }

     void add_object(arm_navigation_msgs::CollisionObject co) {
         //Adds a collision object to the map.
         arm_navigation_msgs::AttachedCollisionObject obj;
         co.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
         obj.object = co;
         publish(obj, collision_object_pub);
     }

     void remove_collision_object(std::string object_id) {
         arm_navigation_msgs::AttachedCollisionObject obj;
         obj.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
         obj.object.header.frame_id = world_frame; //Frame referencia.
         obj.object.header.stamp = ros::Time::now();
         obj.object.id = object_id;
         publish(obj, collision_object_pub);
     }

     void attach_object_to_gripper(std::string arm_name, std::string object_id) {
         //Attaches an object to the robot's end effector.
         // **arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object ot
         // **object_id (string):** The ID of the object to attach

         arm_navigation_msgs::AttachedCollisionObject obj;
         obj.link_name = "r_gripper_r_finger_tip_link"; //Link al que se une (mano derecha)
         obj.object.operation.operation =  arm_navigation_msgs::CollisionObjectOperation::ATTACH_AND_REMOVE_AS_OBJECT;
         obj.object.header.stamp = ros::Time::now();
         obj.object.header.frame_id = world_frame; //Frame referencia.
         obj.object.id = object_id;
         obj.touch_links.push_back("r_gripper_palm_link");
         obj.touch_links.push_back("r_gripper_r_finger_link");
         obj.touch_links.push_back("r_gripper_l_finger_link");
         obj.touch_links.push_back("r_gripper_l_finger_tip_link");
         publish(obj, attached_object_pub);
     }


     void detach_all_objects_from_gripper(std::string arm_name) {
         //Detaches all objects and removes them from the collision space entirely.

         arm_navigation_msgs::AttachedCollisionObject obj;
         obj.object.header.stamp = ros::Time::now();
         obj.object.header.frame_id = world_frame; //Frame referencia.
         obj.link_name = "r_gripper_r_finger_tip_link";
         obj.object.id = "all";
         obj.object.operation.operation =  arm_navigation_msgs::CollisionObjectOperation::REMOVE;
         publish(obj, attached_object_pub);
     }

     void detach_object(std::string arm_name, std::string object_id) {
         //Detaches a single object from the arm and removes it from the collision space entirely.

         arm_navigation_msgs::AttachedCollisionObject obj;
         obj.object.header.frame_id = world_frame; //Frame referencia.
         obj.link_name = "r_gripper_r_finger_tip_link";
         obj.object.header.stamp = ros::Time::now();
         obj.object.id = object_id;
         obj.object.operation.operation =  arm_navigation_msgs::CollisionObjectOperation::REMOVE;
         publish(obj, attached_object_pub);
     }

     void detach_and_add_as_object(std::string arm_name, std::string object_id) {
         //Detaches a single object from the gripper and adds it back to the world at its current location.

         arm_navigation_msgs::AttachedCollisionObject obj;
         obj.object.header.stamp = ros::Time::now();
         obj.object.header.frame_id = world_frame; //Frame referencia.
         obj.link_name = "r_gripper_r_finger_tip_link";
         obj.object.id = object_id;
         obj.object.operation.operation =  arm_navigation_msgs::CollisionObjectOperation::DETACH_AND_ADD_AS_OBJECT;
         publish(obj, attached_object_pub);
     }

     std::vector<arm_navigation_msgs::CollisionObject>  collision_objects() {
         //Returns the collision objects in the world.(not attached collision objects!)

         arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
         arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;

         if(!get_planning_scene.call(planning_scene_req, planning_scene_res)) {
             ROS_WARN("Can't get planning scene. COLL");
         }

         printf("Objetos de la escena:\n");
         for(unsigned int i=0; i<planning_scene_res.planning_scene.collision_objects.size(); i++)
         {
             printf("Nom: %s.\n", planning_scene_res.planning_scene.collision_objects[i].id.c_str());
             ros::Duration(0.2).sleep();
         }
         return planning_scene_res.planning_scene.collision_objects;
      }

     std::vector<arm_navigation_msgs::AttachedCollisionObject> attached_collision_objects() {
          //Returns the attached collision objects in the world.
          arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
          arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;

          if(!get_planning_scene.call(planning_scene_req, planning_scene_res)) {
              ROS_WARN("Can't get planning scene. ATT");
          }

          printf("Agarrados de la escena:\n");
          for(unsigned int i=0; i<planning_scene_res.planning_scene.attached_collision_objects.size(); i++)
          {
              printf("Nom: %s.\n", planning_scene_res.planning_scene.attached_collision_objects[i].object.id.c_str());
              ros::Duration(0.2).sleep();
          }
          return planning_scene_res.planning_scene.attached_collision_objects;
      }
};

Asked by Sanxano on 2012-12-04 06:59:03 UTC

Comments

Answers

hi, your code seems fine. so what is your question?

Asked by yangyangcv on 2012-12-04 13:36:36 UTC

Comments