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 ANTI_COLLISION_BASE_CONTROLLER_H_
00038 #define ANTI_COLLISION_BASE_CONTROLLER_H_
00039
00040 #include <vector>
00041 #include <math.h>
00042 #include <ros/console.h>
00043 #include <angles/angles.h>
00044
00045
00046 #include <costmap_2d/costmap_2d.h>
00047 #include <costmap_2d/costmap_2d_ros.h>
00048
00049 #include <base_local_planner/trajectory.h>
00050 #include <base_local_planner/world_model.h>
00051 #include <base_local_planner/costmap_model.h>
00052
00053
00054 #include <geometry_msgs/PoseStamped.h>
00055 #include <geometry_msgs/Point.h>
00056 #include <geometry_msgs/Twist.h>
00057 #include <nav_msgs/Odometry.h>
00058
00059
00060 #include <tf/transform_datatypes.h>
00061 #include <tf/transform_listener.h>
00062 #include <tf/tf.h>
00063
00064 #include <assisted_teleop/trajectory.h>
00065
00066 namespace anti_collision_base_controller {
00071 class AntiCollisionBaseController{
00072
00073 public:
00077 AntiCollisionBaseController();
00078
00082 ~AntiCollisionBaseController();
00083
00084 tf::TransformListener tf_;
00085
00086 void joyCallBack(const geometry_msgs::TwistConstPtr& msg);
00087
00088 void odomCallback(const nav_msgs::OdometryConstPtr& msg);
00089
00090 void spin();
00091
00092 private:
00093
00111 bool generateTrajectory(double x, double y, double theta,
00112 double vx, double vy, double vtheta,
00113 double vx_samp, double vy_samp, double vtheta_samp,
00114 double acc_x, double acc_y, double acc_theta,
00115 base_local_planner::Trajectory& traj, double sim_time_local);
00123 double footprintCost(double x_i, double y_i, double theta_i);
00124
00125 costmap_2d::Costmap2D costmap_;
00126
00127 std::vector<geometry_msgs::Point> footprint_spec_;
00128
00129 double inscribed_radius_, circumscribed_radius_, inflation_radius_;
00130
00131 double sim_time_;
00132 double sim_granularity_;
00133
00134 int vx_samples_;
00135 int vtheta_samples_;
00136 double acc_lim_x_, acc_lim_y_, acc_lim_theta_;
00137
00138 double max_vel_x_, min_vel_x_, max_vel_th_, min_vel_th_, min_in_place_vel_th_;
00139
00140 double controller_frequency_;
00141
00142 std::string global_frame_, robot_base_frame_;
00143
00153 inline double computeNewXPosition(double xi, double vx, double vy, double theta, double dt){
00154 return xi + (vx * cos(theta) + vy * cos(M_PI_2 + theta)) * dt;
00155 }
00156
00166 inline double computeNewYPosition(double yi, double vx, double vy, double theta, double dt){
00167 return yi + (vx * sin(theta) + vy * sin(M_PI_2 + theta)) * dt;
00168 }
00169
00177 inline double computeNewThetaPosition(double thetai, double vth, double dt){
00178 return thetai + vth * dt;
00179 }
00180
00181
00190 inline double computeNewVelocity(double vg, double vi, double a_max, double dt){
00191 if(vg >= 0)
00192 return std::min(vg, vi + a_max * dt);
00193 return std::max(vg, vi - a_max * dt);
00194 }
00195
00196 base_local_planner::WorldModel* world_model_;
00197
00198 ros::Publisher base_cmd_pub_;
00199
00200 ros::NodeHandle ros_node_;
00201
00202 ros::Subscriber joy_sub_;
00203
00204 ros::Subscriber odom_sub_;
00205
00206 void computeSafeVelocity(double x, double y, double theta, double vx_current, double vy_current, double vtheta_current, double vx_desired, double vy_desired, double vtheta_desired, double &vx_result, double &vy_result, double &vtheta_result);
00207
00208 costmap_2d::Costmap2DROS *costmap_ros_;
00209
00210 geometry_msgs::Twist vel_desired_, base_odom_;
00211 boost::mutex vel_desired_mutex_, base_odom_mutex_;
00212
00213 bool getRobotPose(double &x, double &y, double &theta);
00214
00215 ros::Time last_cmd_received_;
00216
00217 double timeout_;
00218 double min_vel_cmd_x_, min_vel_cmd_y_, min_vel_cmd_theta_;
00219
00220 trajectory::Trajectory *sim_trajectory_;
00221 };
00222 };
00223
00224 #endif