00001 #ifndef _GOTO_CROSSING_CROSSING_GOER_H_ 00002 #define _GOTO_CROSSING_CROSSING_GOER_H_ 00003 00004 #include <cmath> 00005 00006 #include <ros/ros.h> 00007 #include <angles/angles.h> 00008 #include <std_msgs/Bool.h> 00009 #include <std_srvs/Empty.h> 00010 #include <geometry_msgs/Point.h> 00011 #include <geometry_msgs/Twist.h> 00012 00013 #include <lama_msgs/Crossing.h> 00014 00015 00016 namespace goto_crossing 00017 { 00018 00019 class CrossingGoer 00020 { 00021 public: 00022 00023 CrossingGoer(); 00024 00025 bool goto_crossing(const lama_msgs::Crossing& crossing, geometry_msgs::Twist& twist); 00026 void callback_goto_crossing(const lama_msgs::Crossing& crossing); 00027 00028 void resetIntegrals() {sum_v_ = 0; sum_w_ = 0;} 00029 00030 private: 00031 00032 bool goToGoal(const geometry_msgs::Point& goal, geometry_msgs::Twist& twist); 00033 bool callback_resetIntegrals(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); 00034 00035 // Parameters shown outside. 00036 double kp_v_; 00037 double kp_w_; 00038 double ki_v_; 00039 double ki_w_; 00040 double min_linear_velocity_; 00041 double max_linear_velocity_; 00042 00043 double min_angular_velocity_; 00044 double max_angular_velocity_; 00045 00046 double reach_distance_; 00047 double dtheta_force_left_; 00048 00049 00050 00051 double threshold_w_only_; 00052 double max_sum_v_; 00053 double max_sum_w_; 00054 00055 // Internals. 00056 ros::NodeHandle nh_; 00057 ros::Publisher twist_publisher_; 00058 ros::Publisher goal_reached_publisher_; 00059 ros::ServiceServer reset_integral_server_; 00060 ros::Time last_t_; 00061 double sum_v_; 00062 double sum_w_; 00063 }; 00064 00065 } // namespace goto_crossing 00066 00067 #endif // _GOTO_CROSSING_CROSSING_GOER_H_