36 #define BOOST_PARAMETER_MAX_ARITY 7 41 using octomap_msgs::Octomap;
47 DiagnosticNodelet(
"OctomapServerContact"),
48 m_octreeContact(
NULL),
49 m_publishUnknownSpace(false),
50 m_publishFrontierSpace(false),
51 m_offsetVisualizeUnknown(0),
52 m_maxRangeProximity(0.05),
53 m_occupancyMinX(-
std::numeric_limits<double>::
max()),
54 m_occupancyMaxX(
std::numeric_limits<double>::
max()),
55 m_occupancyMinY(-
std::numeric_limits<double>::
max()),
56 m_occupancyMaxY(
std::numeric_limits<double>::
max()),
57 m_useContactSurface(true)
64 ROS_ERROR(
"Could not convert OcTreeContact from OcTree");
83 privateNh.
param(
"color_unknown/r", r, 0.5);
84 privateNh.
param(
"color_unknown/g", g, 0.5);
85 privateNh.
param(
"color_unknown/b", b, 0.7);
86 privateNh.
param(
"color_unknown/a", a, 1.0);
91 privateNh.
param(
"color_frontier/r", r, 1.0);
92 privateNh.
param(
"color_frontier/g", g, 0.0);
93 privateNh.
param(
"color_frontier/b", b, 0.0);
94 privateNh.
param(
"color_frontier/a", a, 1.0);
122 double defaultPadding, defaultScale;
123 privateNh.
param<
double>(
"self_see_defaultPadding", defaultPadding, .01);
124 privateNh.
param<
double>(
"self_see_defaultScale", defaultScale, 1.0);
125 std::vector<robot_self_filter::LinkInfo>
links;
127 if (!privateNh.
hasParam(
"self_see_links")) {
128 ROS_WARN(
"No links specified for self filtering.");
132 privateNh.
getParam(
"self_see_links", sslVals);
134 ROS_WARN(
"Self see links need to be an array");
137 if (sslVals.
size() == 0) {
138 ROS_WARN(
"No values in self see links array");
141 for (
int i = 0; i < sslVals.
size(); i++) {
142 robot_self_filter::LinkInfo li;
144 ROS_WARN(
"Self see links entry %d is not a structure. Stopping processing of self see links",i);
147 if (!sslVals[i].hasMember(
"name")) {
148 ROS_WARN(
"Self see links entry %d has no name. Stopping processing of self see links",i);
151 li.name = std::string(sslVals[i][
"name"]);
152 if (!sslVals[i].hasMember(
"padding")) {
153 ROS_DEBUG(
"Self see links entry %d has no padding. Assuming default padding of %g",i,defaultPadding);
154 li.padding = defaultPadding;
157 li.padding = sslVals[i][
"padding"];
159 if (!sslVals[i].hasMember(
"scale")) {
160 ROS_DEBUG(
"Self see links entry %d has no scale. Assuming default scale of %g",i,defaultScale);
161 li.scale = defaultScale;
164 li.scale = sslVals[i][
"scale"];
189 ROS_ERROR_STREAM(
"Transform error of sensor data: " << ex.what() <<
", quitting callback");
193 Eigen::Matrix4f sensorToWorld;
197 pcl::transformPointCloud(pc, pc, sensorToWorld);
199 pc.header = pc.header;
204 ROS_DEBUG(
"Pointcloud insertion in OctomapServer done (%zu pts, %f sec)", pc.size(), total_elapsed);
210 point3d sensorOrigin = pointTfToOctomap(sensorOriginTf);
218 #ifdef COLOR_OCTOMAP_SERVER 219 unsigned char*
colors =
new unsigned char[3];
223 KeySet free_cells, occupied_cells;
226 for (PCLPointCloud::const_iterator it = pc.begin(); it != pc.end(); ++it) {
238 occupied_cells.insert(key);
243 #ifdef COLOR_OCTOMAP_SERVER // NB: Only read and interpret color if it's an occupied node 244 const int rgb = *
reinterpret_cast<const int*
>(&(it->rgb));
245 colors[0] = ((rgb >> 16) & 0xff);
246 colors[1] = ((rgb >> 8) & 0xff);
247 colors[2] = (rgb & 0xff);
248 m_octree->averageNodeColor(it->x, it->y, it->z, colors[0], colors[1], colors[2]);
268 for(KeySet::iterator it = free_cells.begin(),
end=free_cells.end(); it!=
end; ++it){
269 if (occupied_cells.find(*it) == occupied_cells.end()){
275 for (KeySet::iterator it = occupied_cells.begin(),
end=occupied_cells.end(); it!=
end; it++) {
308 std::vector<jsk_recognition_msgs::ContactSensor>
datas = msg->datas;
312 std_msgs::Header tmpHeader;
314 tmpHeader.stamp = msg->header.stamp;
327 unsigned int steps[3];
329 for (
int i = 0; i < 3; ++i) {
330 diff[i] = pmax(i) - pmin(i);
331 steps[i] = floor(diff[i] / resolution);
337 std::vector< std::vector<bool> > containFlag(datas.size(), std::vector<bool>(8));
339 for (
unsigned int x = 0;
x < steps[0]; ++
x) {
341 for (
unsigned int y = 0;
y < steps[1]; ++
y) {
343 for (
unsigned int z = 0;
z < steps[2]; ++
z) {
347 point3d vertexOffset(-resolution/2.0, -resolution/2.0, -resolution/2.0);
349 for (
int i = 0; i < 2; i++) {
350 if (i == 1) { vertexOffset.
z() += resolution; }
351 for (
int j = 0; j < 2; j++) {
352 if (j == 1) { vertexOffset.
y() += resolution; }
353 for (
int k = 0; k < 2; k++) {
354 if (k == 1) { vertexOffset.
x() += resolution; }
355 vertex = p + vertexOffset;
358 for (
int l=0;
l<datas.size();
l++) {
359 if (
m_selfMask->getMaskContainmentforNamedLink(vertex(0), vertex(1), vertex(2), datas[
l].
link_name) == robot_self_filter::INSIDE) {
361 containFlag[
l][i+j*2+k*4] =
true;
364 containFlag[
l][i+j*2+k*4] =
false;
368 vertexOffset.
x() -= resolution;
370 vertexOffset.
y() -= resolution;
374 std::vector<bool> containFlagLinkSum(8,
false);
375 std::vector<bool> containFlagVerticesSum(datas.size(),
false);
376 std::vector<bool> containFlagVerticesProd(datas.size(),
true);
377 bool insideFlag =
false;
378 bool surfaceFlag =
false;
379 for (
int l = 0;
l < datas.size();
l++) {
380 for (
int i = 0; i < 8; i++) {
381 if (containFlag[
l][i]) {
382 containFlagLinkSum[i] =
true;
383 containFlagVerticesSum[
l] =
true;
386 containFlagVerticesProd[
l] =
false;
390 insideFlag = (std::find(containFlagLinkSum.begin(), containFlagLinkSum.end(),
false) == containFlagLinkSum.end());
391 for (
int l = 0;
l < datas.size();
l++) {
392 if (containFlagVerticesSum[
l] && !(containFlagVerticesProd[
l]) ) {
393 if (datas[l].contact) {
405 else if (surfaceFlag) {
419 point3d vertexOffset(-resolution/2.0, -resolution/2.0, -resolution/2.0);
420 #pragma omp parallel for 421 for (
unsigned int cnt = 0; cnt < steps[0] * steps[1] * steps[2]; ++cnt) {
426 id[0] = cnt / (steps[1] * steps[2]);
427 id[1] = (cnt % (steps[1] * steps[2])) / steps[2];
428 id[2] = (cnt % (steps[1] * steps[2])) % steps[2];
429 p.
x() = pmin(0) + resolution *
id[0];
430 p.
y() = pmin(1) + resolution *
id[1];
431 p.
z() = pmin(2) + resolution *
id[2];
434 vertex = p + vertexOffset;
436 for (
int l=0;
l<datas.size();
l++) {
437 if (
m_selfMask->getMaskContainmentforNamedLink(vertex(0), vertex(1), vertex(2), datas[
l].
link_name) == robot_self_filter::INSIDE) {
462 if (octomapSize <= 1) {
463 ROS_WARN(
"Nothing to publish, octree is empty");
477 visualization_msgs::MarkerArray freeNodesVis;
481 geometry_msgs::Pose
pose;
485 visualization_msgs::MarkerArray occupiedNodesVis;
490 pcl::PointCloud<pcl::PointXYZ> pclCloud;
506 double x = it.getX();
507 double y = it.getY();
508 double z = it.getZ();
510 double size = it.getSize();
511 double x = it.getX();
512 double y = it.getY();
516 ROS_DEBUG(
"Ignoring single speckle at (%f,%f,%f)", x, y, z);
526 if (publishMarkerArray) {
527 unsigned idx = it.getDepth();
528 assert(idx < occupiedNodesVis.markers.size());
530 geometry_msgs::Point cubeCenter;
535 occupiedNodesVis.markers[idx].points.push_back(cubeCenter);
537 double minX, minY, minZ, maxX, maxY, maxZ;
542 occupiedNodesVis.markers[idx].colors.push_back(
heightMapColor(h));
547 if (publishPointCloud) {
548 pclCloud.push_back(pcl::PointXYZ(x, y, z));
553 double x = it.getX();
554 double y = it.getY();
555 double z = it.getZ();
562 double x = it.getX();
563 double y = it.getY();
566 if (publishFreeMarkerArray) {
567 unsigned idx = it.getDepth();
568 assert(idx < freeNodesVis.markers.size());
570 geometry_msgs::Point cubeCenter;
575 freeNodesVis.markers[idx].points.push_back(cubeCenter);
586 if (publishMarkerArray) {
587 for (
unsigned i = 0; i < occupiedNodesVis.markers.size(); ++i) {
591 occupiedNodesVis.markers[i].header.stamp = rostime;
593 occupiedNodesVis.markers[i].id = i;
594 occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
595 occupiedNodesVis.markers[i].scale.x = size;
596 occupiedNodesVis.markers[i].scale.y = size;
597 occupiedNodesVis.markers[i].scale.z = size;
598 occupiedNodesVis.markers[i].color =
m_color;
601 if (occupiedNodesVis.markers[i].points.size() > 0) {
602 occupiedNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
605 occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
614 if (publishFreeMarkerArray) {
615 for (
unsigned i = 0; i < freeNodesVis.markers.size(); ++i) {
619 freeNodesVis.markers[i].header.stamp = rostime;
621 freeNodesVis.markers[i].id = i;
622 freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
623 freeNodesVis.markers[i].scale.x = size;
624 freeNodesVis.markers[i].scale.y = size;
625 freeNodesVis.markers[i].scale.z = size;
629 if (freeNodesVis.markers[i].points.size() > 0) {
630 freeNodesVis.markers[i].action = visualization_msgs::Marker::ADD;
633 freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE;
642 if (publishUnknownMarkerArray) {
643 visualization_msgs::MarkerArray unknownNodesVis;
644 unknownNodesVis.markers.resize(1);
652 pcl::PointCloud<pcl::PointXYZ> unknownCloud;
654 for (point3d_list::iterator it = unknownLeaves.begin(); it != unknownLeaves.end(); it++) {
658 unknownCloud.push_back(pcl::PointXYZ(x, y, z));
660 geometry_msgs::Point cubeCenter;
665 double minX, minY, minZ, maxX, maxY, maxZ;
671 unknownNodesVis.markers[0].points.push_back(cubeCenter);
676 unknownNodesVis.markers[0].header.stamp = rostime;
678 unknownNodesVis.markers[0].id = 0;
679 unknownNodesVis.markers[0].type = visualization_msgs::Marker::CUBE_LIST;
680 unknownNodesVis.markers[0].scale.x = size;
681 unknownNodesVis.markers[0].scale.y = size;
682 unknownNodesVis.markers[0].scale.z = size;
685 if (unknownNodesVis.markers[0].points.size() > 0) {
686 unknownNodesVis.markers[0].action = visualization_msgs::Marker::ADD;
689 unknownNodesVis.markers[0].action = visualization_msgs::Marker::DELETE;
694 sensor_msgs::PointCloud2 unknownRosCloud;
697 unknownRosCloud.header.stamp = rostime;
701 if (publishFrontierMarkerArray) {
702 visualization_msgs::MarkerArray frontierNodesVis;
703 frontierNodesVis.markers.resize(1);
704 pcl::PointCloud<pcl::PointXYZ> frontierCloud;
710 std::vector< std::vector< std::vector<int> > > check_unknown(x_num, std::vector< std::vector<int> >(y_num, std::vector<int>(z_num)));
711 std::vector< std::vector< std::vector<int> > > check_occupied(x_num, std::vector< std::vector<int> >(y_num, std::vector<int>(z_num)));
712 std::vector< std::vector< std::vector<int> > > check_frontier(x_num, std::vector< std::vector<int> >(y_num, std::vector<int>(z_num)));
714 for (
int i = 0; i < x_num; i++) {
715 for (
int j = 0; j < y_num; j++) {
716 for (
int k = 0; k < z_num; k++) {
717 check_unknown[i][j][k] = 0;
718 check_occupied[i][j][k] = 0;
719 check_frontier[i][j][k] = 0;
725 for (point3d_list::iterator it_unknown = unknownLeaves.begin();
726 it_unknown != unknownLeaves.end();
729 double x_unknown = it_unknown->x();
730 double y_unknown = it_unknown->y();
731 double z_unknown = it_unknown->z();
732 int x_index = int(std::round((x_unknown -
m_occupancyMinX) / resolution - 1));
733 int y_index = int(std::round((y_unknown -
m_occupancyMinY) / resolution - 1));
734 int z_index = int(std::round((z_unknown -
m_occupancyMinZ) / resolution - 1));
735 check_unknown[x_index][y_index][z_index] = 1;
739 for (
int idx = 0; idx < occupiedNodesVis.markers.size(); idx++) {
740 double size_occupied = occupiedNodesVis.markers[idx].scale.x;
741 for (
int id = 0;
id < occupiedNodesVis.markers[idx].points.size();
id++) {
742 double x_occupied = occupiedNodesVis.markers[idx].points[id].x;
743 double y_occupied = occupiedNodesVis.markers[idx].points[id].y;
744 double z_occupied = occupiedNodesVis.markers[idx].points[id].z;
745 int x_min_index = std::round((x_occupied - (size_occupied / 2.0) -
m_occupancyMinX) / resolution);
746 int y_min_index = std::round((y_occupied - (size_occupied / 2.0) -
m_occupancyMinY) / resolution);
747 int z_min_index = std::round((z_occupied - (size_occupied / 2.0) -
m_occupancyMinZ) / resolution);
748 for (
int i = x_min_index; i < x_min_index + int(size_occupied/resolution); i++) {
749 for (
int j = y_min_index; j < y_min_index + int(size_occupied/resolution); j++) {
750 for (
int k = z_min_index; k < z_min_index + int(size_occupied/resolution); k++) {
751 check_occupied[i][j][k] = 1;
761 geometry_msgs::Point cubeCenter;
762 for (
int i = 0; i < x_num; i++) {
763 for (
int j = 0; j < y_num; j++) {
764 for (
int k = 0; k < z_num-1; k++) {
765 for (
int l = -1;
l <= 1;
l++) {
766 if ( i+
l < 0 || x_num <= i+
l )
continue;
767 for (
int m = -1; m <= 1; m++) {
768 if ( j+m < 0 || y_num <= j+m )
continue;
769 for (
int n = -1;
n <= 1;
n++) {
770 if ( k+
n < 0 || z_num <= k+
n )
continue;
771 if (
l == 0 && m == 0 &&
n== 0)
continue;
772 if (check_unknown[i+
l][j+m][k+
n] == 1 && check_unknown[i][j][k] == 0 && check_occupied[i][j][k] == 0 && check_frontier[i][j][k] == 0) {
773 check_frontier[i][j][k] = 1;
778 double minX, minY, minZ, maxX, maxY, maxZ;
784 frontierNodesVis.markers[0].points.push_back(cubeCenter);
796 frontierNodesVis.markers[0].header.stamp = rostime;
798 frontierNodesVis.markers[0].id = 0;
799 frontierNodesVis.markers[0].type = visualization_msgs::Marker::CUBE_LIST;
800 frontierNodesVis.markers[0].scale.x = size;
801 frontierNodesVis.markers[0].scale.y = size;
802 frontierNodesVis.markers[0].scale.z = size;
805 if (frontierNodesVis.markers[0].points.size() > 0) {
806 frontierNodesVis.markers[0].action = visualization_msgs::Marker::ADD;
809 frontierNodesVis.markers[0].action = visualization_msgs::Marker::DELETE;
815 sensor_msgs::PointCloud2 frontierRosCloud;
818 frontierRosCloud.header.stamp = rostime;
824 if (publishPointCloud) {
825 sensor_msgs::PointCloud2
cloud;
828 cloud.header.stamp = rostime;
832 if (publishBinaryMap) {
836 if (publishFullMap) {
841 ROS_DEBUG(
"Map publishing in OctomapServer took %f sec", totalElapsed);
846 DiagnosticNodelet::onInit();
double min(const OneDataStat &d)
wrapper function for min method for boost::function
tf::TransformListener m_tfListener
iterator begin(unsigned char maxDepth=0) const
void updateInnerOccupancy()
void publish(const boost::shared_ptr< M > &message) const
octomap::OcTreeKey m_updateBBXMax
std::string m_worldFrameId
unsigned short int coordToKey(double coordinate) const
static void updateMaxKey(const octomap::OcTreeKey &in, octomap::OcTreeKey &max)
octomap::OcTreeKey m_updateBBXMin
virtual OcTreeNode * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
double max(const OneDataStat &d)
wrapper function for max method for boost::function
const iterator end() const
void transformAsMatrix(const tf::Transform &bt, Eigen::Matrix4f &out_mat)
virtual void handleFreeNodeInBBX(const OcTreeT::iterator &it)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
void publishBinaryOctoMap(const ros::Time &rostime=ros::Time::now()) const
bool isInUpdateBBX(const OcTreeT::iterator &it) const
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
Type const & getType() const
void getUnknownLeafCenters(point3d_list &node_centers, point3d pmin, point3d pmax, unsigned int depth=0) const
double keyToCoord(unsigned short int key, unsigned depth) const
static void updateMinKey(const octomap::OcTreeKey &in, octomap::OcTreeKey &min)
bool computeRayKeys(const point3d &origin, const point3d &end, KeyRay &ray) const
TFSIMD_FORCE_INLINE Vector3 normalized() const
static std_msgs::ColorRGBA heightMapColor(double h)
virtual void handlePreNodeTraversal(const ros::Time &rostime)
virtual void handleOccupiedNode(const OcTreeT::iterator &it)
ros::Publisher m_fmarkerPub
bool isSpeckleNode(const octomap::OcTreeKey &key) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
std_msgs::ColorRGBA m_color
virtual void getMetricMin(double &x, double &y, double &z)
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
ros::Publisher m_binaryMapPub
void publishFullOctoMap(const ros::Time &rostime=ros::Time::now()) const
std_msgs::ColorRGBA m_colorFree
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
#define ROS_DEBUG_STREAM(args)
std::list< octomath::Vector3 > point3d_list
virtual void getMetricMax(double &x, double &y, double &z)
#define NODELET_INFO(...)
uint32_t getNumSubscribers() const
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
double getResolution() const
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
virtual void handleNode(const OcTreeT::iterator &it)
bool getParam(const std::string &key, std::string &s) const
ros::Publisher m_pointCloudPub
virtual void handlePostNodeTraversal(const ros::Time &rostime)
double getNodeSize(unsigned depth) const
ros::Publisher m_markerPub
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
ros::Publisher m_fullMapPub
pcl::PointCloud< pcl::PointXYZ > PCLPointCloud
virtual void handleNodeInBBX(const OcTreeT::iterator &it)
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
virtual size_t size() const
virtual void handleFreeNode(const OcTreeT::iterator &it)
virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator &it)
Connection registerCallback(const C &callback)