[hector_exploration_planner]

* Add visualization for exploration transform
* Change color of frontier markers depending on if they're valid or not

hector_exploration_planner/include/hector_exploration_planner/exploration_transform_vis.h

@@ -0,0 +1,104 @@
+//=================================================================================================
+// Copyright (c) 2012, Mark Sollweck, Stefan Kohlbrecher, TU Darmstadt
+// 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 the Simulation, Systems Optimization and Robotics
+//       group, TU Darmstadt 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 HOLDER 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.
+//=================================================================================================
+
+#ifndef EXPLORATION_TRANSFORM_VIS_H___
+#define EXPLORATION_TRANSFORM_VIS_H___
+
+#include <sensor_msgs/PointCloud.h>
+#include <ros/ros.h>
+#include <costmap_2d/costmap_2d.h>
+
+
+namespace hector_exploration_planner{
+
+  class ExplorationTransformVis
+  {
+  public:
+    ExplorationTransformVis(const std::string& topic_name)
+    {
+      ros::NodeHandle pnh("~");
+      exploration_transform_pointcloud_pub_ = pnh.advertise<sensor_msgs::PointCloud>(topic_name, 2, false);
+    }
+
+    virtual ~ExplorationTransformVis()
+    {}
+
+    void publishVisOnDemand(const costmap_2d::Costmap2D& costmap, const unsigned int* exploration_array)
+    {
+      if (exploration_transform_pointcloud_pub_.getNumSubscribers() > 0){
+        unsigned int size_x = costmap.getSizeInCellsX();
+        unsigned int size_y = costmap.getSizeInCellsY();
+        unsigned int size = size_x * size_y;
+
+        unsigned int max = 0;
+
+        for (size_t i = 0; i < size; ++i){
+          if ((exploration_array[i] < INT_MAX) && (exploration_array[i] > max)){
+            max = exploration_array[i];
+          }
+        }
+
+        float max_f = static_cast<float>(max);
+
+        sensor_msgs::PointCloud cloud;
+        cloud.header.frame_id = "/map";
+        cloud.header.stamp = ros::Time::now();
+
+        double x_world, y_world;
+
+        geometry_msgs::Point32 point;
+
+        for (size_t x = 0; x < size_x; ++x){
+          for (size_t y = 0; y < size_y; ++y){
+
+            unsigned int index = costmap.getIndex(x,y);
+
+            if (exploration_array[index] < INT_MAX){
+
+              costmap.mapToWorld(x,y, x_world, y_world);
+              point.x = x_world;
+              point.y = y_world;
+              point.z = static_cast<float>(exploration_array[index])/max_f;
+
+              cloud.points.push_back(point);
+            }
+
+          }
+        }
+        exploration_transform_pointcloud_pub_.publish(cloud);
+      }
+    }
+
+  protected:
+
+    ros::Publisher exploration_transform_pointcloud_pub_;
+  };
+
+}
+
+#endif

hector_exploration_planner/include/hector_exploration_planner/hector_exploration_planner.h

@@ -36,6 +36,8 @@
 
 #include <dynamic_reconfigure/server.h>
 
+#include <hector_exploration_planner/exploration_transform_vis.h>
+
 
 
 namespace hector_exploration_planner{
@@ -159,6 +161,8 @@ class HectorExplorationPlanner {
 
   boost::shared_ptr<dynamic_reconfigure::Server<hector_exploration_planner::ExplorationPlannerConfig> > dyn_rec_server_;
 
+  boost::shared_ptr<ExplorationTransformVis> vis_;
+
 };
 }
 

hector_exploration_planner/src/hector_exploration_planner.cpp

@@ -99,6 +99,8 @@ void HectorExplorationPlanner::initialize(std::string name, costmap_2d::Costmap2
   this->initialized_ = true;
   this->previous_goal_ = -1;
 
+  vis_.reset(new ExplorationTransformVis("exploration_transform"));
+
 }
 
 void HectorExplorationPlanner::dynRecParamCallback(hector_exploration_planner::ExplorationPlannerConfig &config, uint32_t level)
@@ -220,6 +222,9 @@ bool HectorExplorationPlanner::doExploration(const geometry_msgs::PoseStamped &s
   if(!buildexploration_trans_array_(start,goals,true)){
     return false;
   }
+
+  vis_->publishVisOnDemand(costmap_, exploration_trans_array_);
+
   if(!getTrajectory(start,goals,plan)){
     ROS_INFO("[hector_exploration_planner] exploration: could not plan to frontier, starting inner-exploration");
     return doInnerExploration(start,plan);
@@ -1085,6 +1090,7 @@ bool HectorExplorationPlanner::findFrontiers(std::vector<geometry_msgs::PoseStam
     if(isFrontierReached(frontier_point)){
       frontier_is_valid = false;
     }
+
     for(size_t i = 0; i < noFrontiers.size(); ++i){
       const geometry_msgs::PoseStamped& noFrontier = noFrontiers[i];
       unsigned int mx,my;
@@ -1095,44 +1101,54 @@ bool HectorExplorationPlanner::findFrontiers(std::vector<geometry_msgs::PoseStam
       }
     }
 
+    geometry_msgs::PoseStamped finalFrontier;
+    double wx,wy;
+    costmap_.mapToWorld(x,y,wx,wy);
+    std::string global_frame = costmap_ros_->getGlobalFrameID();
+    finalFrontier.header.frame_id = global_frame;
+    finalFrontier.pose.position.x = wx;
+    finalFrontier.pose.position.y = wy;
+    finalFrontier.pose.position.z = 0.0;
+
+    double yaw = getYawToUnknown(costmap_.getIndex(x,y));
+
     if(frontier_is_valid){
-      geometry_msgs::PoseStamped finalFrontier;
-      double wx,wy;
-      costmap_.mapToWorld(x,y,wx,wy);
-      std::string global_frame = costmap_ros_->getGlobalFrameID();
-      finalFrontier.header.frame_id = global_frame;
-      finalFrontier.pose.position.x = wx;
-      finalFrontier.pose.position.y = wy;
-      finalFrontier.pose.position.z = 0.0;
 
-      double yaw = getYawToUnknown(costmap_.getIndex(x,y));
       finalFrontier.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
       frontiers.push_back(finalFrontier);
+    }
 
-      // visualization (export to method?)
-      if(visualization_requested){
-        visualization_msgs::Marker marker;
-        marker.header.frame_id = "map";
-        marker.header.stamp = ros::Time();
-        marker.ns = "hector_exploration_planner";
-        marker.id = id++;
-        marker.type = visualization_msgs::Marker::ARROW;
-        marker.action = visualization_msgs::Marker::ADD;
-        marker.pose.position.x = wx;
-        marker.pose.position.y = wy;
-        marker.pose.position.z = 0.0;
-        marker.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
-        marker.scale.x = 0.5;
-        marker.scale.y = 0.5;
-        marker.scale.z = 0.5;
-        marker.color.a = 1.0;
+    // visualization (export to method?)
+    if(visualization_requested){
+      visualization_msgs::Marker marker;
+      marker.header.frame_id = "map";
+      marker.header.stamp = ros::Time();
+      marker.ns = "hector_exploration_planner";
+      marker.id = id++;
+      marker.type = visualization_msgs::Marker::ARROW;
+      marker.action = visualization_msgs::Marker::ADD;
+      marker.pose.position.x = wx;
+      marker.pose.position.y = wy;
+      marker.pose.position.z = 0.0;
+      marker.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
+      marker.scale.x = 0.2;
+      marker.scale.y = 0.2;
+      marker.scale.z = 0.2;
+      marker.color.a = 1.0;
+
+      if(frontier_is_valid){
+        marker.color.r = 0.0;
+        marker.color.g = 1.0;
+      }else{
         marker.color.r = 1.0;
         marker.color.g = 0.0;
-        marker.color.b = 0.0;
-        marker.lifetime = ros::Duration(5,0);
-        visualization_pub_.publish(marker);
       }
+
+      marker.color.b = 0.0;
+      marker.lifetime = ros::Duration(5,0);
+      visualization_pub_.publish(marker);
     }
+
   }
   return !frontiers.empty();
 }
@@ -1265,9 +1281,9 @@ bool HectorExplorationPlanner::findInnerFrontier(std::vector<geometry_msgs::Pose
         marker.pose.position.y = wfy;
         marker.pose.position.z = 0.0;
         marker.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_path);
-        marker.scale.x = 0.5;
-        marker.scale.y = 0.5;
-        marker.scale.z = 0.5;
+        marker.scale.x = 0.2;
+        marker.scale.y = 0.2;
+        marker.scale.z = 0.2;
         marker.color.a = 1.0;
         marker.color.r = 0.0;
         marker.color.g = 0.0;