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 
38 #include <cstdlib>
39 
40 static const std::string ROBOT_DESCRIPTION = "robot_description";
41 
42 int main(int argc, char** argv)
43 {
44  ros::init(argc, argv, "visualize_robot_collision_volume");
45 
47  spinner.start();
48 
49  double radius = 0.02;
50  double lifetime = 600.0;
51 
54  if (psm.getPlanningScene())
55  {
57  psm.startSceneMonitor();
58  psm.startStateMonitor();
59  ros::NodeHandle nh;
60  ros::Publisher pub_markers = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 10);
61  std::cout << "\nListening for planning scene...\nType the number of spheres to generate and press Enter: "
62  << std::endl;
63  int N;
64  std::cin >> N;
65 
66  planning_scene::PlanningScenePtr scene = psm.getPlanningScene();
67  std::vector<double> aabb;
68  scene->getCurrentState().computeAABB(aabb);
69 
70  // publish the bounding box
71  visualization_msgs::Marker mk;
72  mk.header.stamp = ros::Time::now();
73  mk.header.frame_id = scene->getPlanningFrame();
74  mk.ns = "bounding_box";
75  mk.id = 0;
76  mk.type = visualization_msgs::Marker::CUBE;
77  mk.action = visualization_msgs::Marker::ADD;
78  mk.pose.position.x = (aabb[0] + aabb[1]) / 2.0;
79  mk.pose.position.y = (aabb[2] + aabb[3]) / 2.0;
80  mk.pose.position.z = (aabb[4] + aabb[5]) / 2.0;
81  mk.pose.orientation.w = 1.0;
82  mk.scale.x = aabb[1] - aabb[0];
83  mk.scale.y = aabb[3] - aabb[2];
84  mk.scale.z = aabb[5] - aabb[4];
85  mk.color.r = 0.0f;
86  mk.color.g = 0.5f;
87  mk.color.b = 1.0f;
88  mk.color.a = 0.3f;
89  mk.lifetime = ros::Duration(lifetime);
90  visualization_msgs::MarkerArray arr;
91  arr.markers.push_back(mk);
92  pub_markers.publish(arr);
93 
94  Eigen::Affine3d t;
95  t.setIdentity();
96  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> points;
97  std::size_t published = 0;
100 
101  std_msgs::ColorRGBA color;
102  color.r = 1.0f;
103  color.g = 0.0f;
104  color.b = 0.0f;
105  color.a = 1.0f;
106 
107  for (int i = 0; i < N; ++i)
108  {
109  t.translation() = Eigen::Vector3d(rng.uniformReal(aabb[0], aabb[1]), rng.uniformReal(aabb[2], aabb[3]),
110  rng.uniformReal(aabb[4], aabb[5]));
111  scene->getWorldNonConst()->clearObjects();
112  scene->getWorldNonConst()->addToObject("test", shapes::ShapeConstPtr(new shapes::Sphere(radius)), t);
114  scene->checkCollision(req, res);
115  if (res.collision)
116  {
117  points.push_back(t.translation());
118  if (points.size() - published >= 100 || (points.size() > published && i + 1 >= N))
119  {
120  arr.markers.clear();
121  for (std::size_t k = published; k < points.size(); ++k)
122  {
123  visualization_msgs::Marker mk;
124  mk.header.stamp = ros::Time::now();
125  mk.header.frame_id = scene->getPlanningFrame();
126  mk.ns = "colliding";
127  mk.id = k;
128  mk.type = visualization_msgs::Marker::SPHERE;
129  mk.action = visualization_msgs::Marker::ADD;
130  mk.pose.position.x = points[k].x();
131  mk.pose.position.y = points[k].y();
132  mk.pose.position.z = points[k].z();
133  mk.pose.orientation.w = 1.0;
134  mk.scale.x = mk.scale.y = mk.scale.z = radius;
135  mk.color = color;
136  mk.lifetime = ros::Duration(lifetime);
137  arr.markers.push_back(mk);
138  pub_markers.publish(arr);
139  }
140  pub_markers.publish(arr);
141  published = points.size();
142  }
143  }
144  }
145  }
146 
147  return 0;
148 }
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor.
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
void spinner()
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
void startWorldGeometryMonitor(const std::string &collision_objects_topic=DEFAULT_COLLISION_OBJECT_TOPIC, const std::string &planning_scene_world_topic=DEFAULT_PLANNING_SCENE_WORLD_TOPIC, const bool load_octomap_monitor=true)
Start listening for objects in the world, the collision map and attached collision objects...
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)
void startStateMonitor(const std::string &joint_states_topic=DEFAULT_JOINT_STATES_TOPIC, const std::string &attached_objects_topic=DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC)
Start the current state monitor.
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 Sun Oct 18 2020 13:17:34