Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00045 #include "planning_environment/monitors/planning_monitor.h"
00046 #include <arm_navigation_msgs/CollisionMap.h>
00047
00048 #include <iostream>
00049 #include <sstream>
00050 #include <string>
00051 #include <map>
00052
00053 class RemoveObjectExample
00054 {
00055 public:
00056
00057 RemoveObjectExample(void)
00058 {
00059 collisionModels_ = new planning_environment::CollisionModels("robot_description");
00060 if (collisionModels_->loadedModels())
00061 {
00062 collisionMapPublisher_ = nh_.advertise<arm_navigation_msgs::CollisionMap>("collision_map_with_removed_box", 1);
00063 planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_);
00064 planningMonitor_->setOnAfterMapUpdateCallback(boost::bind(&RemoveObjectExample::afterWorldUpdate, this, _1, _2));
00065 }
00066 }
00067
00068 virtual ~RemoveObjectExample(void)
00069 {
00070 if (planningMonitor_)
00071 delete planningMonitor_;
00072 if (collisionModels_)
00073 delete collisionModels_;
00074 }
00075
00076 protected:
00077
00078 void afterWorldUpdate(const arm_navigation_msgs::CollisionMapConstPtr &collisionMap, bool clear)
00079 {
00080
00081 if (!clear)
00082 return;
00083
00084
00085
00086
00087
00088 planningMonitor_->getEnvironmentModel()->lock();
00089
00090
00091 collision_space::EnvironmentModel *env = planningMonitor_->getEnvironmentModel()->clone();
00092
00093
00094 planningMonitor_->getEnvironmentModel()->unlock();
00095
00096
00097
00098 shapes::Shape *box = new shapes::Box(2, 2, 2);
00099 tf::Transform pose;
00100 pose.setIdentity();
00101
00102
00103 env->removeCollidingObjects(box, pose);
00104
00105
00106 arm_navigation_msgs::CollisionMap cmap;
00107 planningMonitor_->recoverCollisionMap(env, cmap);
00108 collisionMapPublisher_.publish(cmap);
00109
00110
00111 delete env;
00112
00113 ROS_INFO("Received collision map with %d points and published one with %d points",
00114 (int)collisionMap->boxes.size(), (int)cmap.boxes.size());
00115
00116 }
00117
00118 private:
00119
00120 ros::NodeHandle nh_;
00121 tf::TransformListener tf_;
00122
00123 ros::Publisher collisionMapPublisher_;
00124 planning_environment::CollisionModels *collisionModels_;
00125 planning_environment::PlanningMonitor *planningMonitor_;
00126 };
00127
00128 int main(int argc, char **argv)
00129 {
00130 ros::init(argc, argv, "remove_object_example");
00131
00132 RemoveObjectExample example;
00133 ros::spin();
00134
00135 return 0;
00136 }