42 int main(
int argc,
char** argv)
44 ros::init(argc, argv,
"visualize_robot_collision_volume");
50 double lifetime = 600.0;
61 std::cout <<
"\nListening for planning scene...\nType the number of spheres to generate and press Enter: " 67 std::vector<double> aabb;
68 scene->getCurrentState().computeAABB(aabb);
71 visualization_msgs::Marker mk;
73 mk.header.frame_id = scene->getPlanningFrame();
74 mk.ns =
"bounding_box";
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];
90 visualization_msgs::MarkerArray arr;
91 arr.markers.push_back(mk);
96 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> points;
97 std::size_t published = 0;
101 std_msgs::ColorRGBA color;
107 for (
int i = 0; i < N; ++i)
111 scene->getWorldNonConst()->clearObjects();
114 scene->checkCollision(req, res);
117 points.push_back(t.translation());
118 if (points.size() - published >= 100 || (points.size() > published && i + 1 >= N))
121 for (std::size_t k = published; k < points.size(); ++k)
123 visualization_msgs::Marker mk;
125 mk.header.frame_id = scene->getPlanningFrame();
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;
137 arr.markers.push_back(mk);
141 published = points.size();
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()
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)
std::shared_ptr< const Shape > ShapeConstPtr