ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Planning scene [closed]

asked 2012-12-04 05:59:03 -0600

Sanxano gravatar image

updated 2012-12-04 22:00:23 -0600

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.


World_interface world;
  arm_navigation_msgs::CollisionObject pieza1_object; = "pieza1_pole";
  pieza1_object.header.frame_id = "/odom_combined";
    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;
  if(plan_response.error_code.val != plan_response.error_code.SUCCESS) {

class World_interface {

    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

    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;

        if(publisher == attached_object_pub) {
        else {


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

     World_interface() {

         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);

         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() {

     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 ...
edit retag flag offensive reopen merge delete

Closed for the following reason Question does not follow our guidelines for questions. Please see: for more details. by tfoote
close date 2014-10-02 02:56:41.516431

1 Answer

Sort by ยป oldest newest most voted

answered 2012-12-04 12:36:36 -0600

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

edit flag offensive delete link more

Question Tools


Asked: 2012-12-04 05:59:03 -0600

Seen: 526 times

Last updated: Dec 04 '12