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 #ifndef COB_COLLISION_VELOCITY_FILTER_H
00019 #define COB_COLLISION_VELOCITY_FILTER_H
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <ros/ros.h>
00029 #include <XmlRpc.h>
00030
00031 #include <pthread.h>
00032
00033
00034 #include <geometry_msgs/Twist.h>
00035 #include <geometry_msgs/PolygonStamped.h>
00036 #include <geometry_msgs/Polygon.h>
00037 #include <nav_msgs/OccupancyGrid.h>
00038
00039 #include <boost/tokenizer.hpp>
00040 #include <boost/foreach.hpp>
00041 #include <boost/algorithm/string.hpp>
00042
00043
00044 #include "cob_footprint_observer/GetFootprint.h"
00045
00046
00047 #include <dynamic_reconfigure/server.h>
00048 #include <cob_collision_velocity_filter/CollisionVelocityFilterConfig.h>
00049
00050
00051 #include "velocity_limited_marker.h"
00052
00053
00054 #include <tf/transform_listener.h>
00055 #include <costmap_2d/costmap_2d_ros.h>
00061 class CollisionVelocityFilter
00062 {
00063 public:
00064
00068 CollisionVelocityFilter(costmap_2d::Costmap2DROS * costmap);
00069
00073 ~CollisionVelocityFilter();
00074
00080 void joystickVelocityCB(const geometry_msgs::Twist::ConstPtr &twist);
00081
00086 void readObstacles();
00087
00091 void getFootprint(const ros::TimerEvent&);
00092
00098 void dynamicReconfigureCB(const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config,
00099 const uint32_t level);
00100
00102
00103 ros::NodeHandle nh_;
00104
00105
00106 ros::NodeHandle pnh_;
00107
00109 ros::Timer get_footprint_timer_;
00110
00112 ros::Publisher topic_pub_command_;
00113 ros::Publisher topic_pub_relevant_obstacles_;
00114
00116 ros::Subscriber joystick_velocity_sub_, obstacles_sub_;
00117
00119 dynamic_reconfigure::Server<cob_collision_velocity_filter::CollisionVelocityFilterConfig> dyn_server_;
00120 dynamic_reconfigure::Server<cob_collision_velocity_filter::CollisionVelocityFilterConfig>::CallbackType dynCB_;
00121
00122 private:
00123
00124
00125 costmap_2d::Costmap2DROS* anti_collision_costmap_;
00126
00131 void performControllerStep();
00132
00137 void obstacleHandler();
00138
00139
00140
00144 double sign(double x);
00145
00151 double getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b);
00152
00159 bool obstacleValid(double x_obstacle, double y_obstacle);
00160
00164 void stopMovement();
00165
00166 pthread_mutex_t m_mutex;
00167
00168
00169 int costmap_obstacle_treshold_;
00170
00171
00172 std::string global_frame_, robot_frame_;
00173
00174
00175 geometry_msgs::Vector3 robot_twist_linear_, robot_twist_angular_;
00176 double v_max_, vtheta_max_;
00177 double ax_max_, ay_max_, atheta_max_;
00178
00179
00180 std::vector<geometry_msgs::Point> robot_footprint_;
00181 double footprint_left_, footprint_right_, footprint_front_, footprint_rear_;
00182 double footprint_left_initial_, footprint_right_initial_, footprint_front_initial_, footprint_rear_initial_;
00183 bool costmap_received_;
00184 nav_msgs::OccupancyGrid last_costmap_received_, relevant_obstacles_;
00185 double influence_radius_, stop_threshold_, obstacle_damping_dist_, use_circumscribed_threshold_;
00186 double closest_obstacle_dist_, closest_obstacle_angle_;
00187
00188
00189 double last_time_;
00190 double kp_, kv_;
00191 double vx_last_, vy_last_, vtheta_last_;
00192 double virt_mass_;
00193
00194
00195 cob_collision_velocity_filter::VelocityLimitedMarker velocity_limited_marker_;
00196
00197 };
00198
00199
00200 #endif
00201