costmap_trajectory_checker.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  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 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.
 *********************************************************************/

#ifndef COSTMAP_TRAJECTORY_CHECKER_H_
#define COSTMAP_TRAJECTORY_CHECKER_H_

#include <boost/thread.hpp>

#include <string>

#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_publisher.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/costmap_model.h>
#include <geometry_msgs/Pose2D.h>


class CostmapTrajectoryChecker {
public:
        CostmapTrajectoryChecker();

        CostmapTrajectoryChecker(costmap_2d::Costmap2DROS* costmap_ros,
                        std::string frame_id,
                        std::vector<geometry_msgs::Point> footprint,
                        std::string topic = "");

        CostmapTrajectoryChecker(costmap_2d::Costmap2DROS* costmap_ros, std::string topic = "");

        CostmapTrajectoryChecker(const CostmapTrajectoryChecker &checker);
        CostmapTrajectoryChecker& operator=(const CostmapTrajectoryChecker& checker);

        void initialize(costmap_2d::Costmap2DROS* costmap_ros, std::string topic = "");


        ~CostmapTrajectoryChecker();

        void setFootprint(const std::vector<geometry_msgs::Point> &footprint);

        void setFootprint(double length, double width, double x_offset, double y_offset);

        std::vector<geometry_msgs::Point> getFootprint();

        void setGlobalFrameID(std::string frame_id);
        void setRobotFrameID(std::string frame_id);
        std::string getGlobalFrameID();
        std::string getRobotFrameID();

        void setPubTopic(std::string topic);
        std::string getPubTopic();

        static void generateTrajectory(const geometry_msgs::Pose2D &start_pose,
                        const geometry_msgs::Twist &vel, unsigned int num_steps, double dt,
                        std::vector<geometry_msgs::Pose2D>& traj);

        double checkTrajectory(const std::vector<geometry_msgs::Pose2D>& traj, bool update_map = false, bool clear_footprint = false);

        double checkTrajectoryMonotonic(const std::vector<geometry_msgs::Pose2D>& traj, bool update_map = false, bool clear_footprint = false, unsigned int num_consec_points_in_collision = 5);

        double checkTwist(const geometry_msgs::Twist &vel, unsigned int num_steps = 10, double dt = 0.2,
                        bool update_map = false, bool clear_footprint = false);

        double checkTwistMonotonic(const geometry_msgs::Twist &vel, unsigned int num_steps = 10, double dt = 0.2,
                        bool update_map = false, bool clear_footprint = false);
        
        bool clearFootprint(const geometry_msgs::Pose2D &traj, bool update_map);

        bool clearFootprint(bool update_map);

private:
        tf::TransformListener tl_; 
        base_local_planner::WorldModel* world_model_; 
        costmap_2d::Costmap2DROS* costmap_ros_; 
        costmap_2d::Costmap2D costmap_; 

        ros::NodeHandle nh_; 
        ros::Publisher traj_pub_; 
        std::string traj_topic_name_; 

        std::string robot_frame_; 
        std::string global_frame_; 
        double inscribed_radius_, circumscribed_radius_, inflation_radius_;
        std::vector<geometry_msgs::Point> footprint_spec_; 
        bool initialized_;

        double footprintCost(const geometry_msgs::Pose2D &pose);

        bool getRobotPose(geometry_msgs::Pose2D &pose_2d) const;

        void publishTrajectory(const std::vector<geometry_msgs::Pose2D>& path);

        void getOrientedFootprint(const geometry_msgs::Pose2D &pose, std::vector<geometry_msgs::Point> &oriented_footprint);

        static void poseTFToPose2D(const tf::Pose &pose_tf, geometry_msgs::Pose2D &pose_2d);

        static void pose2DToTF(const geometry_msgs::Pose2D pose_2d, tf::Pose &pose_tf);

        static void pose2DToPose(const geometry_msgs::Pose2D pose_2d, geometry_msgs::Pose &pose);

        static void poseToPose2D(const geometry_msgs::Pose pose, geometry_msgs::Pose2D &pose_2d);

        static void integratePose(const geometry_msgs::Twist &vel, double dt,
                        geometry_msgs::Pose2D &pose);
};

#endif // COSTMAP_TRAJECTORY_CHECKER_H_