PPExplorer.h

Go to the documentation of this file.

/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, ISR University of Coimbra.
*  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 ISR University of Coimbra 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.
*
* Author: Gonçalo Cabrita on 5/1/2011
*********************************************************************/
#include <ros/ros.h>
//#include <pcl_ros/subscriber.h>
#include <angles/angles.h>

#include <pcl_ros/point_cloud.h>
#include <pcl/common/common_headers.h>

#include <nav_msgs/GridCells.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/String.h>

#include <tf/transform_listener.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <vector>
#include <list>

#include <pp_explorer/GridCellsIdentified.h>
#include <pp_explorer/StringIdentified.h>
#include <pp_explorer/PoseStampedIdentified.h>

namespace particle_plume
{
        class PPExplorer
        {
                public:
                PPExplorer();
                ~PPExplorer();
                

                bool findYumiestSlice(geometry_msgs::PoseStamped * goal);
                

                bool nextYumiestSlice(geometry_msgs::PoseStamped * goal);
                

                bool findClearing(geometry_msgs::PoseStamped * goal);
                

                double getExploredArea();
                
                bool isReady();
        
                private:
        
                ros::NodeHandle n_;
                ros::NodeHandle pn_;
                message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
                tf::TransformListener tf_;
                tf::MessageFilter<nav_msgs::Odometry> * tf_filter_;
                ros::Subscriber map_sub_;
                ros::Subscriber map_meta_sub_;
                ros::Subscriber cells_sub_;
                ros::Subscriber plume_sub_;
                //pcl_ros::Subscriber<pcl::PointXYZI> plume_sub_;
                ros::Subscriber obstacles_sub_;
                ros::Subscriber inflated_sub_;
                
                // ******* Multi-robot stuff *******
                bool multi_robot_;
                std::string unique_id_;
                ros::Subscriber recovery_sub_;
                ros::Publisher recovery_pub_;
                ros::Subscriber chatter_sub_;
                ros::Publisher chatter_pub_;
                ros::Subscriber poses_sub_;
                ros::Publisher poses_pub_;
                
                std::vector<pp_explorer::GridCellsIdentified> recovery_cells_;
                
                std::vector<pp_explorer::PoseStampedIdentified> other_robots_poses_;
                
                bool finish_on_next_recovery_;
                // *********************************
                
                // ******* Debug stuff *******
                ros::Publisher debug_pub_;
                ros::Publisher debug_goal_pub_;
                
                bool debug_slices_;
                // ***************************
                
                pcl::PointCloud<pcl::PointXYZI> plume_;
                
                nav_msgs::GridCells cells_;
                
                nav_msgs::OccupancyGrid map_;
                nav_msgs::MapMetaData map_metadata_;
                
                nav_msgs::GridCells obstacles_;
                nav_msgs::GridCells inflated_;
                
                geometry_msgs::PoseStamped robot_pose_;
                
                std::string global_frame_id_;
                

                double pie_radius_;

                int pie_slices_;

                double max_visited_cells_coef_;

                double max_odor_visited_cells_coef_;

                int find_clearing_sim_steps_;

                double inflation_radius_;
                

                double visited_cells_weight_;

                double current_heading_weight_;

                double odor_weight_;

                double other_robots_weight_;
                

                double bubble_radius_;

                int min_particle_count_difference_;
                
                struct SliceOfPie
                {
                        geometry_msgs::Pose goal;
                        double yuminess;
                };
                
                std::list<SliceOfPie> best_slices_;
                
                 class CompareSlices
                 {
                        public:
                bool operator()(const SliceOfPie & first, const SliceOfPie & second)
                {
                        if(first.yuminess < second.yuminess) return true;
                                else return false;      
                }
                };
                

                void ppCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
        

                void cellsCallback(const nav_msgs::GridCells::ConstPtr& msg);
                

                void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
                

                void mapMetaCallback(const nav_msgs::MapMetaData::ConstPtr& msg);
                

                void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
                

                void obstaclesCallback(const nav_msgs::GridCells::ConstPtr& msg);
                

                void inflatedCallback(const nav_msgs::GridCells::ConstPtr& msg);
                

                void recoveryCellsCallback(const pp_explorer::GridCellsIdentified::ConstPtr& msg);
                

                void chatterCallback(const pp_explorer::StringIdentified::ConstPtr& msg);
                

                void posesCallback(const pp_explorer::PoseStampedIdentified::ConstPtr& msg);
                
                

                void setGoal(geometry_msgs::PoseStamped * goal);
                

                bool testCell(geometry_msgs::Point * cell, std::vector<geometry_msgs::Point> * cells);
                

                bool yumiestSlice(geometry_msgs::PoseStamped * goal, geometry_msgs::PoseStamped * robot_pose);
                

                bool isInsidePieRadius(double x, double y, geometry_msgs::PoseStamped * robot);
                

                bool isInsideTheMap(int x, int y);
                

                bool isViableGlobalMapCell(int x, int y);
                

                bool isViableLocalMapGoal(double x, double y);
                

                void explorationComplete();
                
                bool got_map_;
                bool got_odom_;
        };
}

// EOF