18 #ifndef COB_COLLISION_VELOCITY_FILTER_H 19 #define COB_COLLISION_VELOCITY_FILTER_H 34 #include <geometry_msgs/Twist.h> 35 #include <geometry_msgs/PolygonStamped.h> 36 #include <geometry_msgs/Polygon.h> 37 #include <nav_msgs/OccupancyGrid.h> 39 #include <boost/tokenizer.hpp> 40 #include <boost/foreach.hpp> 41 #include <boost/algorithm/string.hpp> 44 #include "cob_footprint_observer/GetFootprint.h" 47 #include <dynamic_reconfigure/server.h> 48 #include <cob_collision_velocity_filter/CollisionVelocityFilterConfig.h> 80 void joystickVelocityCB(
const geometry_msgs::Twist::ConstPtr &twist);
98 void dynamicReconfigureCB(
const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config,
99 const uint32_t level);
119 dynamic_reconfigure::Server<cob_collision_velocity_filter::CollisionVelocityFilterConfig>
dyn_server_;
120 dynamic_reconfigure::Server<cob_collision_velocity_filter::CollisionVelocityFilterConfig>::CallbackType
dynCB_;
144 double sign(
double x);
151 double getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b);
ros::Publisher topic_pub_relevant_obstacles_
std::vector< geometry_msgs::Point > robot_footprint_
ros::Timer get_footprint_timer_
Timer for periodically calling GetFootprint Service.
ros::Subscriber joystick_velocity_sub_
declaration of subscriber
dynamic_reconfigure::Server< cob_collision_velocity_filter::CollisionVelocityFilterConfig >::CallbackType dynCB_
double use_circumscribed_threshold_
geometry_msgs::Vector3 robot_twist_angular_
void stopMovement()
stops movement of the robot
dynamic_reconfigure::Server< cob_collision_velocity_filter::CollisionVelocityFilterConfig > dyn_server_
dynamic reconfigure
int costmap_obstacle_treshold_
geometry_msgs::Vector3 robot_twist_linear_
double footprint_rear_initial_
ros::Publisher topic_pub_command_
declaration of publisher
double footprint_front_initial_
double closest_obstacle_dist_
costmap_2d::Costmap2DROS * anti_collision_costmap_
double footprint_left_initial_
ros::Subscriber obstacles_sub_
cob_collision_velocity_filter::VelocityLimitedMarker velocity_limited_marker_
checks for obstacles in driving direction and stops the robot
void obstacleHandler()
checks for obstacles in driving direction of the robot (rotation included) and publishes relevant obs...
nav_msgs::OccupancyGrid relevant_obstacles_
std::string global_frame_
void performControllerStep()
checks distance to obstacles in driving direction and slows down/stops robot and publishes command ve...
nav_msgs::OccupancyGrid last_costmap_received_
double getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b)
computes distance between two points
double closest_obstacle_angle_
double obstacle_damping_dist_
double sign(double x)
returns the sign of x
double footprint_right_initial_
bool obstacleValid(double x_obstacle, double y_obstacle)
checks if obstacle lies already within footprint -> this is ignored due to sensor readings of the hul...
ros::NodeHandle nh_
create a handle for this node, initialize node