$search
00001 /**************************************************************** 00002 * 00003 * Copyright (c) 2012 00004 * 00005 * Fraunhofer Institute for Manufacturing Engineering 00006 * and Automation (IPA) 00007 * 00008 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00009 * 00010 * Project name: care-o-bot 00011 * ROS stack name: cob_navigation 00012 * ROS package name: cob_collision_velocity_filter 00013 * 00014 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00015 * 00016 * Author: Matthias Gruhler, email:Matthias.Gruhler@ipa.fhg.de 00017 * 00018 * Date of creation: February 2012 00019 * 00020 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00021 * 00022 * Redistribution and use in source and binary forms, with or without 00023 * modification, are permitted provided that the following conditions are met: 00024 * 00025 * * Redistributions of source code must retain the above copyright 00026 * notice, this list of conditions and the following disclaimer. 00027 * * Redistributions in binary form must reproduce the above copyright 00028 * notice, this list of conditions and the following disclaimer in the 00029 * documentation and/or other materials provided with the distribution. 00030 * * Neither the name of the Fraunhofer Institute for Manufacturing 00031 * Engineering and Automation (IPA) nor the names of its 00032 * contributors may be used to endorse or promote products derived from 00033 * this software without specific prior written permission. 00034 * 00035 * This program is free software: you can redistribute it and/or modify 00036 * it under the terms of the GNU Lesser General Public License LGPL as 00037 * published by the Free Software Foundation, either version 3 of the 00038 * License, or (at your option) any later version. 00039 * 00040 * This program is distributed in the hope that it will be useful, 00041 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00042 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00043 * GNU Lesser General Public License LGPL for more details. 00044 * 00045 * You should have received a copy of the GNU Lesser General Public 00046 * License LGPL along with this program. 00047 * If not, see <http://www.gnu.org/licenses/>. 00048 * 00049 ****************************************************************/ 00050 #ifndef COB_COLLISION_VELOCITY_FILTER_H 00051 #define COB_COLLISION_VELOCITY_FILTER_H 00052 00053 //################## 00054 //#### includes #### 00055 00056 // standard includes 00057 //-- 00058 00059 // ROS includes 00060 #include <ros/ros.h> 00061 #include <XmlRpc.h> 00062 00063 #include <pthread.h> 00064 00065 // ROS message includes 00066 #include <geometry_msgs/Twist.h> 00067 #include <geometry_msgs/PolygonStamped.h> 00068 #include <geometry_msgs/Polygon.h> 00069 #include <nav_msgs/GridCells.h> 00070 00071 #include <boost/tokenizer.hpp> 00072 #include <boost/foreach.hpp> 00073 #include <boost/algorithm/string.hpp> 00074 00075 // ROS service includes 00076 #include "cob_footprint_observer/GetFootprint.h" 00077 00078 // dynamic reconfigure includes 00079 #include <dynamic_reconfigure/server.h> 00080 #include <cob_collision_velocity_filter/CollisionVelocityFilterConfig.h> 00081 00082 // BUT velocity limited marker 00083 #include "velocity_limited_marker.h" 00084 00090 class CollisionVelocityFilter 00091 { 00092 public: 00093 00097 CollisionVelocityFilter(); 00098 00099 00103 ~CollisionVelocityFilter(); 00104 00110 void joystickVelocityCB(const geometry_msgs::Twist::ConstPtr &twist); 00111 00116 void obstaclesCB(const nav_msgs::GridCells::ConstPtr &obstacles); 00117 00118 00122 void getFootprintServiceCB(const ros::TimerEvent&); 00123 00129 void dynamicReconfigureCB(const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config, 00130 const uint32_t level); 00131 00132 00134 ros::NodeHandle nh_; 00135 00137 ros::Timer get_footprint_timer_; 00138 00140 ros::Publisher topic_pub_command_; 00141 ros::Publisher topic_pub_relevant_obstacles_; 00142 00144 ros::Subscriber joystick_velocity_sub_, obstacles_sub_; 00145 00147 ros::ServiceClient srv_client_get_footprint_; 00148 00150 dynamic_reconfigure::Server<cob_collision_velocity_filter::CollisionVelocityFilterConfig> dyn_server_; 00151 dynamic_reconfigure::Server<cob_collision_velocity_filter::CollisionVelocityFilterConfig>::CallbackType dynCB_; 00152 00153 private: 00154 /* core functions */ 00155 00160 void performControllerStep(); 00161 00166 void obstacleHandler(); 00167 00168 00169 /* helper functions */ 00170 00176 std::vector<geometry_msgs::Point> loadRobotFootprint(ros::NodeHandle node); 00177 00178 00182 double sign(double x); 00183 00189 double getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b); 00190 00197 bool obstacleValid(double x_obstacle, double y_obstacle); 00198 00202 void stopMovement(); 00203 00204 pthread_mutex_t m_mutex; 00205 00206 //frames 00207 std::string global_frame_, robot_frame_; 00208 00209 //velocity 00210 geometry_msgs::Vector3 robot_twist_linear_, robot_twist_angular_; 00211 double v_max_, vtheta_max_; 00212 double ax_max_, ay_max_, atheta_max_; 00213 00214 //obstacle avoidence 00215 std::vector<geometry_msgs::Point> robot_footprint_; 00216 double footprint_left_, footprint_right_, footprint_front_, footprint_rear_; 00217 double footprint_left_initial_, footprint_right_initial_, footprint_front_initial_, footprint_rear_initial_; 00218 bool costmap_received_; 00219 nav_msgs::GridCells last_costmap_received_, relevant_obstacles_; 00220 double influence_radius_, stop_threshold_, obstacle_damping_dist_, use_circumscribed_threshold_; 00221 double closest_obstacle_dist_, closest_obstacle_angle_; 00222 00223 // variables for slow down behaviour 00224 double last_time_; 00225 double kp_, kv_; 00226 double vx_last_, vy_last_, vtheta_last_; 00227 double virt_mass_; 00228 00229 // BUT velocity limited marker 00230 cob_collision_velocity_filter::VelocityLimitedMarker velocity_limited_marker_; 00231 00232 }; //CollisionVelocityFilter 00233 00234 #endif 00235