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

Revision history [back]

try the following code:

include <ros ros.h="">

include <planning_environment models="" collision_models.h="">

include <arm_navigation_msgs planningscene.h="">

include <arm_navigation_msgs getplanningscene.h="">

include <geometric_shapes shape_operations.h="">

include <planning_environment util="" construct_object.h="">

static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";

class PlanningSceneTest { public: PlanningSceneTest() { collision_object_pub_ = nh_.advertise<arm_navigation_msgs::collisionobject>("collision_object", 10); cm_ = new planning_environment::CollisionModels("robot_description"); kinematic_state_ = new planning_models::KinematicState(cm_->getKinematicModel()); kinematic_state_->setKinematicStateToDefault(); while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0))) { ROS_INFO_STREAM("Waiting for set_planning_scene_diff service " << SET_PLANNING_SCENE_DIFF_NAME); } get_planning_scene_client_ = nh_.serviceClient<arm_navigation_msgs::getplanningscene> (SET_PLANNING_SCENE_DIFF_NAME); } ~PlanningSceneTest() {

}

bool getPlanningScene()
{
    arm_navigation_msgs::CollisionObject collision_objects;
    collision_objects.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
    collision_objects.header.stamp = ros::Time::now();
    collision_objects.header.frame_id = "/" + cm_->getWorldFrameId();
    collision_objects.id = "testObj";

    geometry_msgs::Pose cylinderPose;
    cylinderPose.position.x = 0.4;
    cylinderPose.position.y = 0.4;
    cylinderPose.position.z = 0.95;//0.93
    cylinderPose.orientation.x = 0;
    cylinderPose.orientation.y = 0;
    cylinderPose.orientation.z = 0;
    cylinderPose.orientation.w = 1;
    arm_navigation_msgs::Shape cylinder_shape;
    cylinder_shape.type = arm_navigation_msgs::Shape::CYLINDER;
    cylinder_shape.dimensions.resize(2);
    cylinder_shape.dimensions[0] = 0.028;//radius
    cylinder_shape.dimensions[1] = 0.12;//height
    collision_objects.shapes.push_back(cylinder_shape);
    collision_objects.poses.push_back(cylinderPose);

    collision_object_pub_.publish(collision_objects);

    //must call the set_planning_scene_diff service to notify other nodes
    if(kinematic_state_ != NULL)
    {
      ROS_INFO("Reverting planning scene to default.");
      cm_->revertPlanningScene(kinematic_state_);
      kinematic_state_ = NULL;
    }
    arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
    arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
    if(!get_planning_scene_client_.call(planning_scene_req, planning_scene_res)) {
        ROS_ERROR("Can't get planning scene");
        return false;
    }
    kinematic_state_ = cm_->setPlanningScene(planning_scene_res.planning_scene);
    if(kinematic_state_ == NULL)
    {
      ROS_ERROR("Something wrong with planning scene");
      return false;
    }

    return true;
}

ros::NodeHandle nh_;
ros::ServiceClient get_planning_scene_client_;
ros::Publisher collision_object_pub_;

planning_environment::CollisionModels* cm_;
planning_models::KinematicState* kinematic_state_;

};

int main(int argc, char** argv) { ros::init(argc, argv, "CollisionObjectPub", ros::init_options::NoSigintHandler);

PlanningSceneTest* pTest = new PlanningSceneTest();
ros::Duration(5.0).sleep();//must sleep, otherwise the published info may get lost

pTest->getPlanningScene();
ros::waitForShutdown();
return 0;

}

try the following code:

include <ros ros.h="">

include <planning_environment models="" collision_models.h="">

include <arm_navigation_msgs planningscene.h="">

include <arm_navigation_msgs getplanningscene.h="">

include <geometric_shapes shape_operations.h="">

include <planning_environment util="" construct_object.h="">

#include <ros/ros.h>
#include <planning_environment/models/collision_models.h>
#include <arm_navigation_msgs/PlanningScene.h>
#include <arm_navigation_msgs/GetPlanningScene.h>
#include <geometric_shapes/shape_operations.h>
#include <planning_environment/util/construct_object.h>

static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";

"/environment_server/set_planning_scene_diff"; class PlanningSceneTest { public: PlanningSceneTest() { collision_object_pub_ = nh_.advertise<arm_navigation_msgs::collisionobject>("collision_object", nh_.advertise<arm_navigation_msgs::CollisionObject>("collision_object", 10); cm_ = new planning_environment::CollisionModels("robot_description"); kinematic_state_ = new planning_models::KinematicState(cm_->getKinematicModel()); kinematic_state_->setKinematicStateToDefault(); while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0))) { ROS_INFO_STREAM("Waiting for set_planning_scene_diff service " << SET_PLANNING_SCENE_DIFF_NAME); } get_planning_scene_client_ = nh_.serviceClient<arm_navigation_msgs::getplanningscene> nh_.serviceClient<arm_navigation_msgs::GetPlanningScene> (SET_PLANNING_SCENE_DIFF_NAME); } ~PlanningSceneTest() {

{

    }

 bool getPlanningScene()
{
    {
        arm_navigation_msgs::CollisionObject collision_objects;
     collision_objects.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
     collision_objects.header.stamp = ros::Time::now();
     collision_objects.header.frame_id = "/" + cm_->getWorldFrameId();
     collision_objects.id = "testObj";

     geometry_msgs::Pose cylinderPose;
     cylinderPose.position.x = 0.4;
     cylinderPose.position.y = 0.4;
     cylinderPose.position.z = 0.95;//0.93
     cylinderPose.orientation.x = 0;
     cylinderPose.orientation.y = 0;
     cylinderPose.orientation.z = 0;
     cylinderPose.orientation.w = 1;
     arm_navigation_msgs::Shape cylinder_shape;
     cylinder_shape.type = arm_navigation_msgs::Shape::CYLINDER;
     cylinder_shape.dimensions.resize(2);
     cylinder_shape.dimensions[0] = 0.028;//radius
     cylinder_shape.dimensions[1] = 0.12;//height
     collision_objects.shapes.push_back(cylinder_shape);
     collision_objects.poses.push_back(cylinderPose);

     collision_object_pub_.publish(collision_objects);

     //must call the set_planning_scene_diff service to notify other nodes
     if(kinematic_state_ != NULL)
    {
    {
          ROS_INFO("Reverting planning scene to default.");
       cm_->revertPlanningScene(kinematic_state_);
       kinematic_state_ = NULL;
     }
     arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
     arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
     if(!get_planning_scene_client_.call(planning_scene_req, planning_scene_res)) {
         ROS_ERROR("Can't get planning scene");
         return false;
     }
     kinematic_state_ = cm_->setPlanningScene(planning_scene_res.planning_scene);
     if(kinematic_state_ == NULL)
    {
    {
          ROS_ERROR("Something wrong with planning scene");
       return false;
     }

     return true;
 }

 ros::NodeHandle nh_;
 ros::ServiceClient get_planning_scene_client_;
 ros::Publisher collision_object_pub_;

 planning_environment::CollisionModels* cm_;
 planning_models::KinematicState* kinematic_state_;

};

}; int main(int argc, char** argv) { ros::init(argc, argv, "CollisionObjectPub", ros::init_options::NoSigintHandler);

ros::init_options::NoSigintHandler);

    PlanningSceneTest* pTest = new PlanningSceneTest();
 ros::Duration(5.0).sleep();//must sleep, otherwise the published info may get lost

 pTest->getPlanningScene();
 ros::waitForShutdown();
 return 0;
}

}

the tutorial you mentioned almost works except some minor changes.

try the following code:

#include <ros/ros.h>
#include <planning_environment/models/collision_models.h>
#include <arm_navigation_msgs/PlanningScene.h>
#include <arm_navigation_msgs/GetPlanningScene.h>
#include <geometric_shapes/shape_operations.h>
#include <planning_environment/util/construct_object.h>

static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";

class PlanningSceneTest
{
public:
    PlanningSceneTest()
    {
        collision_object_pub_  = nh_.advertise<arm_navigation_msgs::CollisionObject>("collision_object", 10);
        cm_ = new planning_environment::CollisionModels("robot_description");
        kinematic_state_ = new planning_models::KinematicState(cm_->getKinematicModel());
        kinematic_state_->setKinematicStateToDefault();
        while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0)))
        {
          ROS_INFO_STREAM("Waiting for set_planning_scene_diff service " << SET_PLANNING_SCENE_DIFF_NAME);
        }
        get_planning_scene_client_ = nh_.serviceClient<arm_navigation_msgs::GetPlanningScene> (SET_PLANNING_SCENE_DIFF_NAME);
    }
    ~PlanningSceneTest()
    {

    }

    bool getPlanningScene()
    {
        arm_navigation_msgs::CollisionObject collision_objects;
        collision_objects.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
        collision_objects.header.stamp = ros::Time::now();
        collision_objects.header.frame_id = "/" + cm_->getWorldFrameId();
        collision_objects.id = "testObj";

        geometry_msgs::Pose cylinderPose;
        cylinderPose.position.x = 0.4;
        cylinderPose.position.y = 0.4;
        cylinderPose.position.z = 0.95;//0.93
        cylinderPose.orientation.x = 0;
        cylinderPose.orientation.y = 0;
        cylinderPose.orientation.z = 0;
        cylinderPose.orientation.w = 1;
        arm_navigation_msgs::Shape cylinder_shape;
        cylinder_shape.type = arm_navigation_msgs::Shape::CYLINDER;
        cylinder_shape.dimensions.resize(2);
        cylinder_shape.dimensions[0] = 0.028;//radius
        cylinder_shape.dimensions[1] = 0.12;//height
        collision_objects.shapes.push_back(cylinder_shape);
        collision_objects.poses.push_back(cylinderPose);

        collision_object_pub_.publish(collision_objects);

        //must call the set_planning_scene_diff service to notify other nodes
        if(kinematic_state_ != NULL)
        {
          ROS_INFO("Reverting planning scene to default.");
          cm_->revertPlanningScene(kinematic_state_);
          kinematic_state_ = NULL;
        }
        arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
        arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
        if(!get_planning_scene_client_.call(planning_scene_req, planning_scene_res)) {
            ROS_ERROR("Can't get planning scene");
            return false;
        }
        kinematic_state_ = cm_->setPlanningScene(planning_scene_res.planning_scene);
        if(kinematic_state_ == NULL)
        {
          ROS_ERROR("Something wrong with planning scene");
          return false;
        }

        return true;
    }

    ros::NodeHandle nh_;
    ros::ServiceClient get_planning_scene_client_;
    ros::Publisher collision_object_pub_;

    planning_environment::CollisionModels* cm_;
    planning_models::KinematicState* kinematic_state_;

};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "CollisionObjectPub", ros::init_options::NoSigintHandler);

    PlanningSceneTest* pTest = new PlanningSceneTest();
    ros::Duration(5.0).sleep();//must sleep, otherwise the published info may get lost

    pTest->getPlanningScene();
    ros::waitForShutdown();
    return 0;
}

in rviz, you need to add display type "Marker", and subscribe to the topic "/planning_scene_markers"