safe_trajectory_planner_ros.h

Go to the documentation of this file.

/*
 * safe_trajectory_planner.h
 *
 *  Created on: Mar 25, 2010
 *      Author: duhadway
 */

#ifndef SAFE_TRAJECTORY_PLANNER_ROS_H_
#define SAFE_TRAJECTORY_PLANNER_ROS_H_

#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/world_model.h>
#include <base_local_planner/point_grid.h>
#include <base_local_planner/costmap_model.h>
#include <base_local_planner/voxel_grid_model.h>
#include <safe_teleop_base/safe_trajectory_planner.h>

#include <tf/transform_datatypes.h>

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>

#include <tf/transform_listener.h>

#include <boost/thread.hpp>

#include <string>

#include <angles/angles.h>

#include <nav_core/base_local_planner.h>


namespace safe_teleop {

#if ROS_VERSION_MINIMUM(1, 14, 0) // ROS_MELODIC
typedef tf2_ros::Buffer TFListener;
#else
typedef tf::TransformListener TFListener;
#endif

  class SafeTrajectoryPlannerROS {
    public:
      SafeTrajectoryPlannerROS(TFListener* tf, costmap_2d::Costmap2DROS* costmap_ros);

      ~SafeTrajectoryPlannerROS();

      bool computeVelocityCommands(const geometry_msgs::Twist::ConstPtr& vel, geometry_msgs::Twist& cmd_vel);

    private:
      bool stopped();

      double distance(double x1, double y1, double x2, double y2);

      bool transformGlobalPlan(std::vector<geometry_msgs::PoseStamped>& transformed_plan);

      void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub, double r, double g, double b, double a);

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

      void cmdCallback(const geometry_msgs::Twist::ConstPtr& vel);

      std::vector<double> loadYVels(ros::NodeHandle node);

      ros::NodeHandle nh_;

      base_local_planner::WorldModel* world_model_; 
      SafeTrajectoryPlanner* tc_; 
      costmap_2d::Costmap2DROS* costmap_ros_; 
      costmap_2d::Costmap2D costmap_; 
      TFListener* tf_; 
      std::string global_frame_; 
      double max_sensor_range_; 
      nav_msgs::Odometry base_odom_; 
      std::string robot_base_frame_; 
      double rot_stopped_velocity_, trans_stopped_velocity_;
      double inscribed_radius_, circumscribed_radius_;
      ros::Publisher l_plan_pub_;
      ros::Publisher u_plan_pub_;
      ros::Subscriber odom_sub_, user_sub_;

      ros::Publisher cmd_pub_;
      ros::Subscriber cmd_sub_;

      boost::recursive_mutex odom_lock_;
      double max_vel_th_, min_vel_th_;
  };

}


#endif /* SAFE_TRAJECTORY_PLANNER_ROS_H_ */