Working again, and working wayyyy faster

Author: ericperko

octocostmap/include/octocostmap/costmap_3d.h

@@ -37,6 +37,7 @@
 
 #include <ros/ros.h>
 #include <tf/transform_listener.h>
+#include <tf/transform_datatypes.h>
 #include <octomap/octomap.h>
 #include <octomap_server/OctomapBinary.h>
 #include <geometry_msgs/PointStamped.h>
@@ -50,7 +51,7 @@ namespace octocostmap {
 
                 void octomapCallback(const octomap_server::OctomapBinary::ConstPtr& map);
 
-                bool checkCollisionVolume(const std::vector<geometry_msgs::PointStamped> &collision_volume);
+                bool checkCollisionVolume(const std::vector<tf::Point > &collision_volume);
 
                 bool checkRectangularPrismBase(const geometry_msgs::PointStamped &origin, double width, double height, double length, double resolution);
             private:
@@ -60,6 +61,7 @@ namespace octocostmap {
                 ros::NodeHandle nh_;
                 ros::NodeHandle priv_nh_;
                 ros::Subscriber octomap_sub_;
+                ros::Publisher collision_volume_pub_;
                 octomap::OcTree octree_;
                 boost::shared_mutex octree_lock_;
         };

octocostmap/src/costmap_3d.cpp

@@ -35,11 +35,14 @@
 #include <octocostmap/costmap_3d.h>
 #include <octomap_server/octomap_server.h>
 #include <boost/foreach.hpp>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
 
 namespace octocostmap {
   Costmap3D::Costmap3D(const std::string &name, tf::TransformListener &tfl):
-    name_(name), tfl_(tfl), nh_(), priv_nh_("~/" + name_), octree_(0.5), map_frame_("map") {
+    name_(name), map_frame_("map"), tfl_(tfl), nh_(), priv_nh_("~/" + name_), octree_(0.5) {
       octomap_sub_ = nh_.subscribe<octomap_server::OctomapBinary>("octomap", 1, boost::bind(&Costmap3D::octomapCallback, this, _1));
+      collision_volume_pub_ = nh_.advertise<pcl::PointCloud<pcl::PointXYZ> >("collison_volume_cloud", 1);
     }
 
   void Costmap3D::octomapCallback(const octomap_server::OctomapBinary::ConstPtr& map) {
@@ -52,45 +55,65 @@ namespace octocostmap {
   }
 
   bool Costmap3D::checkRectangularPrismBase(const geometry_msgs::PointStamped &origin, double width, double height, double length, double resolution) {
-    std::vector<geometry_msgs::PointStamped> collision_pts;
-    geometry_msgs::PointStamped temp = origin;
+    std::vector<tf::Point > collision_pts;
+    tf::Stamped<tf::Point > temp_origin;
+    tf::pointStampedMsgToTF(origin, temp_origin);
+    tf::StampedTransform transform;
     double num_pts = width * height * length / (resolution * resolution * resolution);
-    collision_pts.reserve( (size_t) (num_pts + 1));
-    for (double dx = 0.0; dx < length; dx += resolution) {
-      for (double dy = 0.0; dy < width; dy += resolution) {
-        for (double dz = 0.0; dz < height; dz += resolution) {
-          temp.point.x = origin.point.x + dx;
-          temp.point.y = origin.point.y + dy;
-          temp.point.z = origin.point.z + dz;
-          collision_pts.push_back(temp);
+    try {
+      tfl_.lookupTransform(map_frame_, temp_origin.frame_id_, temp_origin.stamp_, transform);
+      tf::Point origin_pt = transform * temp_origin;
+      tf::Transform rotation_only(transform.getRotation());
+      tf::Vector3 x_vec = rotation_only * tf::Vector3(length,0,0);
+      tf::Vector3 y_vec = rotation_only * tf::Vector3(0,width,0);
+      tf::Vector3 z_vec = rotation_only * tf::Vector3(0,0,height);
+      tf::Point temp;
+      collision_pts.reserve( (size_t) (num_pts + 1));
+      double x_resolution = resolution/length;
+      double y_resolution = resolution/width;
+      double z_resolution = resolution/height;
+      for (double dx = 0.0; dx <= 1.0; dx += x_resolution) {
+        for (double dy = 0.0; dy <= 1.0; dy += y_resolution) {
+          for (double dz = 0.0; dz <= 1.0; dz += z_resolution) {
+            /*temp.setX(temp_origin.x() + dx);
+            temp.setY(temp_origin.y() + dy);
+            temp.setZ(temp_origin.z() + dz);
+            */
+            temp = origin_pt + dx*x_vec + dy*y_vec + dz*z_vec;
+            collision_pts.push_back(temp);
+          }
+        }
+      }
+      if(collision_volume_pub_.getNumSubscribers() > 0) {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
+        cloud_ptr->header.frame_id = map_frame_;
+        cloud_ptr->header.stamp = temp_origin.stamp_;
+        BOOST_FOREACH( tf::Point pt, collision_pts) {
+          cloud_ptr->push_back(pcl::PointXYZ(pt.x(), pt.y(), pt.z()));
         }
+        collision_volume_pub_.publish(cloud_ptr);
       }
+      return checkCollisionVolume(collision_pts);
+    } catch (tf::TransformException ex) {
+      ROS_ERROR("%s", ex.what());
+      //Err on the side of caution if we can't transform a point
+      return true;
     }
-    return checkCollisionVolume(collision_pts);
   }
 
-  bool Costmap3D::checkCollisionVolume(const std::vector<geometry_msgs::PointStamped> &collision_volume) {
+  bool Costmap3D::checkCollisionVolume(const std::vector<tf::Point> &collision_volume) {
     uint32_t num_points_checked = 0;
     bool retval = false;
-    geometry_msgs::PointStamped temp;
     octomap::OcTreeNode *current_cell;
     octree_lock_.lock_shared();
-    BOOST_FOREACH( geometry_msgs::PointStamped pt, collision_volume) {
-      try {
-        tfl_.transformPoint(map_frame_, pt, temp);
-        current_cell = octree_.search(temp.point.x, temp.point.y, temp.point.z);
-        num_points_checked++;
-        if(current_cell) {
-          if (current_cell->isOccupied()) {
-            retval = true;
-            break;
-          }
+    BOOST_FOREACH( tf::Point pt, collision_volume) {
+      current_cell = octree_.search(pt.x(), pt.y(), pt.z());
+      num_points_checked++;
+      if(current_cell) {
+        if (current_cell->isOccupied()) {
+          retval = true;
+          break;
         }
-      } catch (tf::TransformException ex) {
-        ROS_ERROR("%s", ex.what());
-        //Err on the side of caution if we can't transform a point
-        retval = true;
-        break;
       }
     }
     octree_lock_.unlock_shared();

octocostmap/src/costmap_3d_tester.cpp

@@ -0,0 +1,69 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Eric Perko
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Eric Perko nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <geometry_msgs/PointStamped.h>
+#include <octocostmap/costmap_3d.h>
+
+void timerCallback(octocostmap::Costmap3D *costmap, const ros::TimerEvent& event) {
+  ROS_DEBUG("Last callback took %f seconds", event.profile.last_duration.toSec());
+  geometry_msgs::PointStamped origin;
+  origin.header.frame_id = "base_link";
+  origin.header.stamp = ros::Time::now();
+  origin.point.x = -0.711;
+  origin.point.y = -0.3048;
+  origin.point.z = 0.0;
+  double width = 0.6096;
+  double length = 1.422;
+  double height = 2.00;
+  double resolution = 0.05;
+
+  if (costmap->checkRectangularPrismBase(origin, width, height, length, resolution)) {
+        ROS_DEBUG("collision detected");
+  }
+}
+
+int main(int argc, char *argv[]) {
+  ros::init(argc,argv, "costmap_3d_tester");
+  ros::NodeHandle nh;
+  tf::TransformListener tfl;
+  octocostmap::Costmap3D costmap("costmap", tfl);
+
+  ros::Timer timer = nh.createTimer(ros::Duration(0.1), boost::bind(timerCallback, &costmap, _1));
+
+  ros::MultiThreadedSpinner spinner(4); // Use 4 threads
+  spinner.spin(); // spin() will not return until the node has been shutdown
+  return 0;
+}