visualize_robot_collision_volume.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
39 #include <memory>
40 
41 static const std::string ROBOT_DESCRIPTION = "robot_description";
42 
43 int main(int argc, char** argv)
44 {
45  ros::init(argc, argv, "visualize_robot_collision_volume");
46 
48  spinner.start();
49 
50  double radius = 0.02;
51  double lifetime = 600.0;
52  ros::NodeHandle nh;
53 
54  std::shared_ptr<tf2_ros::Buffer> tf_buffer = std::make_shared<tf2_ros::Buffer>();
55  std::shared_ptr<tf2_ros::TransformListener> tf_listener =
56  std::make_shared<tf2_ros::TransformListener>(*tf_buffer, nh);
58  if (psm.getPlanningScene())
59  {
60  psm.startWorldGeometryMonitor();
61  psm.startSceneMonitor();
62  psm.startStateMonitor();
63  ros::Publisher pub_markers = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 10);
64  std::cout << "\nListening for planning scene...\nType the number of spheres to generate and press Enter: "
65  << std::endl;
66  int num_spheres;
67  std::cin >> num_spheres;
68 
69  planning_scene::PlanningScenePtr scene = psm.getPlanningScene();
70  std::vector<double> aabb;
71  scene->getCurrentState().computeAABB(aabb);
72 
73  // publish the bounding box
74  visualization_msgs::Marker mk;
75  mk.header.stamp = ros::Time::now();
76  mk.header.frame_id = scene->getPlanningFrame();
77  mk.ns = "bounding_box";
78  mk.id = 0;
79  mk.type = visualization_msgs::Marker::CUBE;
80  mk.action = visualization_msgs::Marker::ADD;
81  mk.pose.position.x = (aabb[0] + aabb[1]) / 2.0;
82  mk.pose.position.y = (aabb[2] + aabb[3]) / 2.0;
83  mk.pose.position.z = (aabb[4] + aabb[5]) / 2.0;
84  mk.pose.orientation.w = 1.0;
85  mk.scale.x = aabb[1] - aabb[0];
86  mk.scale.y = aabb[3] - aabb[2];
87  mk.scale.z = aabb[5] - aabb[4];
88  mk.color.r = 0.0f;
89  mk.color.g = 0.5f;
90  mk.color.b = 1.0f;
91  mk.color.a = 0.3f;
92  mk.lifetime = ros::Duration(lifetime);
93  visualization_msgs::MarkerArray arr;
94  arr.markers.push_back(mk);
95  pub_markers.publish(arr);
96 
97  Eigen::Isometry3d t;
98  t.setIdentity();
99  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> points;
100  std::size_t published = 0;
103 
104  std_msgs::ColorRGBA color;
105  color.r = 1.0f;
106  color.g = 0.0f;
107  color.b = 0.0f;
108  color.a = 1.0f;
109 
110  for (int i = 0; i < num_spheres; ++i)
111  {
112  t.translation() = Eigen::Vector3d(rng.uniformReal(aabb[0], aabb[1]), rng.uniformReal(aabb[2], aabb[3]),
113  rng.uniformReal(aabb[4], aabb[5]));
114  scene->getWorldNonConst()->clearObjects();
115  scene->getWorldNonConst()->addToObject("test", shapes::ShapeConstPtr(new shapes::Sphere(radius)), t);
117  scene->checkCollision(req, res);
118  if (res.collision)
119  {
120  points.push_back(t.translation());
121  if (points.size() - published >= 100 || (points.size() > published && i + 1 >= num_spheres))
122  {
123  arr.markers.clear();
124  for (std::size_t k = published; k < points.size(); ++k)
125  {
126  visualization_msgs::Marker mk;
127  mk.header.stamp = ros::Time::now();
128  mk.header.frame_id = scene->getPlanningFrame();
129  mk.ns = "colliding";
130  mk.id = k;
131  mk.type = visualization_msgs::Marker::SPHERE;
132  mk.action = visualization_msgs::Marker::ADD;
133  mk.pose.position.x = points[k].x();
134  mk.pose.position.y = points[k].y();
135  mk.pose.position.z = points[k].z();
136  mk.pose.orientation.w = 1.0;
137  mk.scale.x = mk.scale.y = mk.scale.z = radius;
138  mk.color = color;
139  mk.lifetime = ros::Duration(lifetime);
140  arr.markers.push_back(mk);
141  pub_markers.publish(arr);
142  }
143  pub_markers.publish(arr);
144  published = points.size();
145  }
146  }
147  }
148  }
149 
150  return 0;
151 }
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
geometry_msgs::TransformStamped t
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
void spinner()
void publish(const boost::shared_ptr< M > &message) const
PlanningSceneMonitor Subscribes to the topic planning_scene.
double uniformReal(double lower_bound, double upper_bound)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
tf2_ros::Buffer * tf_buffer
static const std::string ROBOT_DESCRIPTION
int main(int argc, char **argv)
static Time now()
std::shared_ptr< const Shape > ShapeConstPtr


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Tue Sep 8 2020 04:15:10