Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
00038 #define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
00039
00040 #include <ros/ros.h>
00041 #include <costmap_2d/costmap_2d.h>
00042 #include <costmap_2d/costmap_2d_publisher.h>
00043 #include <costmap_2d/costmap_2d_ros.h>
00044 #include <base_local_planner/world_model.h>
00045 #include <base_local_planner/point_grid.h>
00046 #include <base_local_planner/costmap_model.h>
00047 #include <base_local_planner/voxel_grid_model.h>
00048 #include <base_local_planner/trajectory_planner.h>
00049 #include <base_local_planner/map_grid_visualizer.h>
00050
00051 #include <base_local_planner/planar_laser_scan.h>
00052
00053 #include <tf/transform_datatypes.h>
00054
00055 #include <nav_msgs/Odometry.h>
00056 #include <geometry_msgs/PoseStamped.h>
00057 #include <geometry_msgs/Twist.h>
00058 #include <geometry_msgs/Point.h>
00059
00060 #include <tf/transform_listener.h>
00061
00062 #include <boost/thread.hpp>
00063
00064 #include <string>
00065
00066 #include <angles/angles.h>
00067
00068 #include <nav_core/base_local_planner.h>
00069
00070 #include <dynamic_reconfigure/server.h>
00071 #include <base_local_planner/BaseLocalPlannerConfig.h>
00072
00073 #include <base_local_planner/odometry_helper_ros.h>
00074
00075 namespace base_local_planner {
00080 class TrajectoryPlannerROS : public nav_core::BaseLocalPlanner {
00081 public:
00085 TrajectoryPlannerROS();
00086
00093 TrajectoryPlannerROS(std::string name,
00094 tf::TransformListener* tf,
00095 costmap_2d::Costmap2DROS* costmap_ros);
00096
00103 void initialize(std::string name, tf::TransformListener* tf,
00104 costmap_2d::Costmap2DROS* costmap_ros);
00105
00109 ~TrajectoryPlannerROS();
00110
00117 bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
00118
00124 bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
00125
00130 bool isGoalReached();
00131
00143 bool checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);
00144
00156 double scoreTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);
00157
00158 bool isInitialized() {
00159 return initialized_;
00160 }
00161
00163 TrajectoryPlanner* getPlanner() const { return tc_; }
00164
00165 private:
00169 void reconfigureCB(BaseLocalPlannerConfig &config, uint32_t level);
00170
00179 bool rotateToGoal(const tf::Stamped<tf::Pose>& global_pose, const tf::Stamped<tf::Pose>& robot_vel, double goal_th, geometry_msgs::Twist& cmd_vel);
00180
00188 bool stopWithAccLimits(const tf::Stamped<tf::Pose>& global_pose, const tf::Stamped<tf::Pose>& robot_vel, geometry_msgs::Twist& cmd_vel);
00189
00190 std::vector<double> loadYVels(ros::NodeHandle node);
00191
00192 double sign(double x){
00193 return x < 0.0 ? -1.0 : 1.0;
00194 }
00195
00196 WorldModel* world_model_;
00197 TrajectoryPlanner* tc_;
00198
00199 costmap_2d::Costmap2DROS* costmap_ros_;
00200 costmap_2d::Costmap2D* costmap_;
00201 MapGridVisualizer map_viz_;
00202 tf::TransformListener* tf_;
00203 std::string global_frame_;
00204 double max_sensor_range_;
00205 nav_msgs::Odometry base_odom_;
00206 std::string robot_base_frame_;
00207 double rot_stopped_velocity_, trans_stopped_velocity_;
00208 double xy_goal_tolerance_, yaw_goal_tolerance_, min_in_place_vel_th_;
00209 std::vector<geometry_msgs::PoseStamped> global_plan_;
00210 bool prune_plan_;
00211 boost::recursive_mutex odom_lock_;
00212
00213 double max_vel_th_, min_vel_th_;
00214 double acc_lim_x_, acc_lim_y_, acc_lim_theta_;
00215 double sim_period_;
00216 bool rotating_to_goal_;
00217 bool reached_goal_;
00218 bool latch_xy_goal_tolerance_, xy_tolerance_latch_;
00219
00220 ros::Publisher g_plan_pub_, l_plan_pub_;
00221
00222 dynamic_reconfigure::Server<BaseLocalPlannerConfig> *dsrv_;
00223 base_local_planner::BaseLocalPlannerConfig default_config_;
00224 bool setup_;
00225
00226
00227 bool initialized_;
00228 base_local_planner::OdometryHelperRos odom_helper_;
00229
00230 std::vector<geometry_msgs::Point> footprint_spec_;
00231 };
00232 };
00233 #endif