search_for_planes.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * 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 Willow Garage, Inc. 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 <occupancy_grid_utils/geometry.h>
#include <occupancy_grid_utils/shortest_path.h>
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>
#include <boost/math/constants/constants.hpp>
#include <geometry_msgs/PolygonStamped.h>

namespace semanticmodel
{
namespace gu=occupancy_grid_utils;
namespace nm=nav_msgs;
namespace vm=visualization_msgs;
namespace gm=geometry_msgs;
namespace al=actionlib;
namespace mbm=move_base_msgs;

using std::set;
using std::vector;
using boost::optional;
using std::cerr;
using std::endl;

typedef boost::mutex::scoped_lock Lock;
typedef std::set<gu::Cell> Cells;


class Node
{
public:
  
  Node ();  
  
  // Store map
  void mapCB (const nm::OccupancyGrid& map);

  // Overall main function that goes through waypoints
  void run ();
  
  // Publish visualization
  void visualize(const ros::WallTimerEvent& e);
  
  // Update which targets are seen given current pose
  void updateSeen(const ros::TimerEvent&e = ros::TimerEvent());
    
private:

  gm::Pose getCurrentPose ();

  gm::Polygon getVisibleRegion (const gm::Pose& p) const;
  
  optional<size_t> getNextGoal();

  bool navigateWaypoint (size_t i);

  optional<size_t> getViewpoint(size_t target);

  ros::NodeHandle nh_;
  boost::mutex mutex_;

  const float robot_radius_;
  
  // Map of the environment
  boost::optional<nm::OccupancyGrid> map_;
  
  // Points we want to try to look at
  vector<gu::Cell> targets_;
  
  // Which targets have and haven't been seen yet
  set<size_t> seen_, unseen_;
  
  // Potential places to look from
  vector<gm::Pose> view_poses_;
  
  // Precomputation of which targets are visible from where
  vector<vector<bool> > is_visible_;
  
  // Which view poses are forbidden, either because we've been
  // there before, or they're not reachable
  set<size_t> forbidden_;
  
  // Flag for node state being initialized
  bool initialized_;
  

  al::SimpleActionClient<mbm::MoveBaseAction> mb_client_;
  ros::Publisher vis_pub_;
  ros::Publisher visible_poly_pub_;
  ros::Subscriber map_sub_;
  ros::WallTimer vis_timer_;
  ros::Timer update_timer_;
  tf::TransformListener tf_;
};

Node::Node () :
  robot_radius_(0.7), mb_client_("move_base", true),
  vis_pub_(nh_.advertise<vm::Marker>("visualization_marker", 100)),
  visible_poly_pub_(nh_.advertise<gm::PolygonStamped>("visible_polygon", 100)),
  map_sub_(nh_.subscribe("/map", 10, &Node::mapCB, this)),
  vis_timer_(nh_.createWallTimer(ros::WallDuration(1.0), &Node::visualize,
                                 this)),
  update_timer_(nh_.createTimer(ros::Duration(0.3), &Node::updateSeen, this))
{}


void Node::mapCB (const nm::OccupancyGrid& map)
{
  Lock l(mutex_);
  map_ = map;
  ROS_INFO ("Map received");
}

// Functor used to discretize reachable subset of grid
struct CheckReachable
{
  CheckReachable (const nm::MapMetaData& info, gu::ResultPtr navfn,
                  float max_dist) :
    info(info), navfn(navfn), max_dist(max_dist)
  {}

  bool operator() (const gu::Cell& c) const
  {
    boost::optional<double> dist = gu::distanceTo(navfn, c);
    return (dist && (*dist < max_dist));
  }

  nm::MapMetaData info;
  gu::ResultPtr navfn;
  float max_dist;
  
};

void Node::run ()
{
  // Wait for the map
  while (ros::ok())
  {
    ros::WallDuration(1.0).sleep();
    Lock l(mutex_);
    if (map_)
      break;
    ROS_INFO ("Waiting for map");
  }
  ROS_INFO("Generating targets and viewpoints");
  
  // Get current pose
  const gm::Pose pose = getCurrentPose();
  const gu::Cell current_cell = gu::pointCell(map_->info, pose.position);
  
  // Get potential view positions: reachable cells that aren't too far away
  nm::OccupancyGrid::Ptr inflated = gu::inflateObstacles(*map_, robot_radius_);
  gu::ResultPtr nav = gu::singleSourceShortestPaths(*inflated, current_cell);
  Cells view_positions = gu::tileCells(map_->info, 0.5,
                                       CheckReachable(map_->info, nav, 7));
  
  // Get targets: reachable cells (in uninflated grid) that aren't too far away
  gu::ResultPtr nav2 = gu::singleSourceShortestPaths(*map_, current_cell);
  Cells target_set = gu::tileCells(map_->info, 0.3,
                                   CheckReachable(map_->info, nav2, 7));

  {
    // Acquire lock before modifying state
    Lock l(mutex_);
    targets_.insert(targets_.end(), target_set.begin(), target_set.end());

    // Initially all targets are unseen
    for (size_t i=0; i<targets_.size(); i++)
      unseen_.insert(i);

    // For each view position, we consider a bunch of orientations
    const float pi = boost::math::constants::pi<float>();
    BOOST_FOREACH (const gu::Cell& pos, view_positions) 
    {
      for (float yaw=0; yaw < 2*pi; yaw+=pi/2)
      {
        gm::Pose view_pose;
        view_pose.position = cellCenter(map_->info, pos);
        view_pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
        view_poses_.push_back(view_pose);
      }
    }

    // Precompute visibility relation between viewpoints and targets
    ROS_INFO ("Computing visibility relation");
    is_visible_.resize(view_poses_.size());
    for (size_t i=0; i<view_poses_.size(); i++)
    {
      const gm::Polygon poly = getVisibleRegion(view_poses_[i]);
      is_visible_[i].resize(targets_.size());
      Cells visible_positions = gu::cellsInConvexPolygon(map_->info, poly);
      for (size_t j=0; j<targets_.size(); j++)
      {
        if (visible_positions.find(targets_[j])!=visible_positions.end())
        {
          is_visible_[i][j] = true;
        }
      }
    }

    ROS_INFO ("Targets generated");
    initialized_ = true;
  }
  
  // Repeatedly pick a navigation waypoint, try to reach it
  // Stop when there's no valid waypoint any more
  while (ros::ok())
  {
    boost::optional<size_t> next_goal;
    {
      Lock l(mutex_);
      next_goal = getNextGoal();
      if (!next_goal)
      {
        ROS_INFO ("No more valid goals; terminating");
        break;
      }
    }
    ROS_INFO ("Navigating to %zu", *next_goal);
    const bool success = navigateWaypoint(*next_goal);
    if (success)
      ROS_INFO("Navigation succeeded");
    else
      ROS_INFO("Navigation failed");
    forbidden_.insert(*next_goal);
  }
}


optional<size_t> Node::getViewpoint (const size_t target)
{
  for (size_t i=0; i<view_poses_.size(); i++)
  {
    if (forbidden_.find(i)==forbidden_.end())
    {
      if (is_visible_[i][target])
        return i;
    }
  }
  return optional<size_t>();
}

optional<size_t> Node::getNextGoal ()
{
  optional<size_t> g;
  BOOST_FOREACH (const size_t target, unseen_)
  {
    // cerr << "Considering target " << target << endl;
    g = getViewpoint(target);
    if (g)
      break;
  }
  return g;
}

bool Node::navigateWaypoint (const size_t wp)
{
  mbm::MoveBaseGoal goal;
  goal.target_pose.header.frame_id = "/map";
  goal.target_pose.header.stamp = ros::Time::now();
  goal.target_pose.pose = view_poses_[wp];

  al::SimpleClientGoalState state =
    mb_client_.sendGoalAndWait(goal, ros::Duration(30.0));
  return (state == al::SimpleClientGoalState::StateEnum::SUCCEEDED);
}

gm::Pose Node::getCurrentPose ()
{
  while (ros::ok())
  {
    if (tf_.waitForTransform("map", "base_footprint", ros::Time(),
                             ros::Duration(1.0)))
      break;
    ROS_INFO ("Waiting for robot pose");
  }
  tf::StampedTransform base_transform;
  tf_.lookupTransform("map", "base_footprint", ros::Time(), base_transform);
  gm::Pose pose;
  tf::poseTFToMsg(base_transform, pose);
  return pose;
  
}

gm::Polygon Node::getVisibleRegion (const gm::Pose& p) const
{
  vector<btVector3> poly(3);
  poly[0] = btVector3(0.5, 1.0, 0.0);
  poly[1] = btVector3(2.0, 0.0, 0.0);
  poly[2] = btVector3(0.5, -1.0, 0.0);

  tf::Pose trans;
  tf::poseMsgToTF(p, trans);

  gm::Polygon polygon;
  BOOST_FOREACH (const btVector3& p, poly) 
  {
    btVector3 p2 = (trans*p).asBt();
    gm::Point32 pt;
    pt.x = p2.x();
    pt.y = p2.y();
    pt.z = p2.z();
    polygon.points.push_back(pt);
  }
  return polygon;
}

void Node::updateSeen (const ros::TimerEvent& e)
{
  if (!initialized_)
    return;
  
  const gm::Pose pose = getCurrentPose();
  gm::PolygonStamped visible_region;
  visible_region.polygon = getVisibleRegion(pose);
  auto visible_cells = gu::cellsInConvexPolygon(map_->info,
                                                visible_region.polygon);
  visible_region.header.frame_id = "/map";
  visible_region.header.stamp = ros::Time::now();
  visible_poly_pub_.publish(visible_region);
  Lock l(mutex_);
  set<size_t> now_seen;
  BOOST_FOREACH (const size_t i, unseen_) 
  {
    if (visible_cells.find(targets_[i])!=visible_cells.end())
      now_seen.insert(i);
  }

  BOOST_FOREACH (const size_t i, now_seen) 
  {
    unseen_.erase(i);
    seen_.insert(i);
  }
}


void Node::visualize (const ros::WallTimerEvent& e)
{
  Lock l(mutex_);
  int id=0;
  if (targets_.size()==0)
    return; // Not initialized yet
  
  vm::Marker seen;
  seen.ns = "search_planes";
  seen.header.frame_id = "/map";
  seen.id = id++;
  seen.type = vm::Marker::SPHERE_LIST;
  seen.scale.x = seen.scale.y = seen.scale.z = 0.04;
  seen.color.b = seen.color.a = 1.0;
  
  vm::Marker unseen = seen;
  unseen.id = id++;
  unseen.color.r = 1.0;
  unseen.color.b = 0.0;
  
  BOOST_FOREACH (const size_t i, seen_) 
    seen.points.push_back(cellCenter(map_->info, targets_[i]));
  vis_pub_.publish(seen);
  
  BOOST_FOREACH (const size_t i, unseen_) 
    unseen.points.push_back(cellCenter(map_->info, targets_[i]));
  vis_pub_.publish(unseen);
  
  
  for (size_t i=0; i<view_poses_.size(); i++)
  {
    if (forbidden_.find(i)!=forbidden_.end())
      continue;
    
    vm::Marker m;
    m.ns = "search_planes";
    m.header.frame_id = "/map";
    m.id = id++;
    m.type = vm::Marker::ARROW;
    m.scale.x = 0.7;
    m.scale.z = m.scale.z = 0.5;
    m.color.g = m.color.a = 1.0;
    m.pose = view_poses_[i];
    vis_pub_.publish(m);
  }
}


} // namespace

int main (int argc, char** argv)
{
  ros::init(argc, argv, "search_for_planes");
  semanticmodel::Node node;
  ros::AsyncSpinner spinner(2);
  spinner.start();
  node.run();
  return 0;
}