Public Member Functions | Public Attributes | Private Member Functions | Private Attributes | List of all members
CollisionVelocityFilter Class Reference

checks for obstacles in driving direction and stops the robot More...

#include <velocity_limited_marker.h>

Public Member Functions

 CollisionVelocityFilter (costmap_2d::Costmap2DROS *costmap)
 Constructor. More...
 
void dynamicReconfigureCB (const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config, const uint32_t level)
 Dynamic reconfigure callback. More...
 
void getFootprint (const ros::TimerEvent &)
 Timer callback, calls GetFootprint Service and adjusts footprint. More...
 
void joystickVelocityCB (const geometry_msgs::Twist::ConstPtr &twist)
 reads twist command from teleop device (joystick, teleop_keyboard, ...) and calls functions for collision check (obstacleHandler) and driving of the robot (performControllerStep) More...
 
void readObstacles ()
 reads obstacles from costmap More...
 
 ~CollisionVelocityFilter ()
 Destructor. More...
 

Public Attributes

dynamic_reconfigure::Server< cob_collision_velocity_filter::CollisionVelocityFilterConfig > dyn_server_
 dynamic reconfigure More...
 
dynamic_reconfigure::Server< cob_collision_velocity_filter::CollisionVelocityFilterConfig >::CallbackType dynCB_
 
ros::Timer get_footprint_timer_
 Timer for periodically calling GetFootprint Service. More...
 
ros::Subscriber joystick_velocity_sub_
 declaration of subscriber More...
 
ros::NodeHandle nh_
 create a handle for this node, initialize node More...
 
ros::Subscriber obstacles_sub_
 
ros::NodeHandle pnh_
 
ros::Publisher topic_pub_command_
 declaration of publisher More...
 
ros::Publisher topic_pub_relevant_obstacles_
 

Private Member Functions

double getDistance2d (geometry_msgs::Point a, geometry_msgs::Point b)
 computes distance between two points More...
 
void obstacleHandler ()
 checks for obstacles in driving direction of the robot (rotation included) and publishes relevant obstacles More...
 
bool obstacleValid (double x_obstacle, double y_obstacle)
 checks if obstacle lies already within footprint -> this is ignored due to sensor readings of the hull etc More...
 
void performControllerStep ()
 checks distance to obstacles in driving direction and slows down/stops robot and publishes command velocity to robot More...
 
double sign (double x)
 returns the sign of x More...
 
void stopMovement ()
 stops movement of the robot More...
 

Private Attributes

costmap_2d::Costmap2DROS * anti_collision_costmap_
 
double atheta_max_
 
double ax_max_
 
double ay_max_
 
double closest_obstacle_angle_
 
double closest_obstacle_dist_
 
int costmap_obstacle_treshold_
 
bool costmap_received_
 
double footprint_front_
 
double footprint_front_initial_
 
double footprint_left_
 
double footprint_left_initial_
 
double footprint_rear_
 
double footprint_rear_initial_
 
double footprint_right_
 
double footprint_right_initial_
 
std::string global_frame_
 
double influence_radius_
 
double kp_
 
double kv_
 
nav_msgs::OccupancyGrid last_costmap_received_
 
double last_time_
 
pthread_mutex_t m_mutex
 
double obstacle_damping_dist_
 
nav_msgs::OccupancyGrid relevant_obstacles_
 
std::vector< geometry_msgs::Point > robot_footprint_
 
std::string robot_frame_
 
geometry_msgs::Vector3 robot_twist_angular_
 
geometry_msgs::Vector3 robot_twist_linear_
 
double stop_threshold_
 
double use_circumscribed_threshold_
 
double v_max_
 
cob_collision_velocity_filter::VelocityLimitedMarker velocity_limited_marker_
 
double virt_mass_
 
double vtheta_last_
 
double vtheta_max_
 
double vx_last_
 
double vy_last_
 

Detailed Description

checks for obstacles in driving direction and stops the robot

Constructor & Destructor Documentation

CollisionVelocityFilter::CollisionVelocityFilter ( costmap_2d::Costmap2DROS *  costmap)

Constructor.

Definition at line 30 of file cob_collision_velocity_filter.cpp.

CollisionVelocityFilter::~CollisionVelocityFilter ( )

Destructor.

Definition at line 149 of file cob_collision_velocity_filter.cpp.

Member Function Documentation

void CollisionVelocityFilter::dynamicReconfigureCB ( const cob_collision_velocity_filter::CollisionVelocityFilterConfig &  config,
const uint32_t  level 
)

Dynamic reconfigure callback.

Parameters
config- configuration file with dynamic reconfigureable parameters
level- the result of ORing together all level values of the parameters that have changed, for now unnecessary

Definition at line 206 of file cob_collision_velocity_filter.cpp.

double CollisionVelocityFilter::getDistance2d ( geometry_msgs::Point  a,
geometry_msgs::Point  b 
)
private

computes distance between two points

Parameters
a,b- Points
Returns
distance

Definition at line 574 of file cob_collision_velocity_filter.cpp.

void CollisionVelocityFilter::getFootprint ( const ros::TimerEvent &  event)

Timer callback, calls GetFootprint Service and adjusts footprint.

Definition at line 175 of file cob_collision_velocity_filter.cpp.

void CollisionVelocityFilter::joystickVelocityCB ( const geometry_msgs::Twist::ConstPtr &  twist)

reads twist command from teleop device (joystick, teleop_keyboard, ...) and calls functions for collision check (obstacleHandler) and driving of the robot (performControllerStep)

Parameters
twist- velocity command sent as twist message (twist.linear.x/y/z, twist.angular.x/y/z)

Definition at line 156 of file cob_collision_velocity_filter.cpp.

void CollisionVelocityFilter::obstacleHandler ( )
private

checks for obstacles in driving direction of the robot (rotation included) and publishes relevant obstacles

Definition at line 356 of file cob_collision_velocity_filter.cpp.

bool CollisionVelocityFilter::obstacleValid ( double  x_obstacle,
double  y_obstacle 
)
private

checks if obstacle lies already within footprint -> this is ignored due to sensor readings of the hull etc

Parameters
x_obstacle- x coordinate of obstacle in occupancy grid local costmap
y_obstacle- y coordinate of obstacle in occupancy grid local costmap
Returns
true if obstacle outside of footprint

Definition at line 587 of file cob_collision_velocity_filter.cpp.

void CollisionVelocityFilter::performControllerStep ( )
private

checks distance to obstacles in driving direction and slows down/stops robot and publishes command velocity to robot

Definition at line 234 of file cob_collision_velocity_filter.cpp.

void CollisionVelocityFilter::readObstacles ( )

reads obstacles from costmap

Parameters
obstacles- 2D occupancy grid in rolling window mode!
double CollisionVelocityFilter::sign ( double  x)
private

returns the sign of x

Definition at line 579 of file cob_collision_velocity_filter.cpp.

void CollisionVelocityFilter::stopMovement ( )
private

stops movement of the robot

Definition at line 599 of file cob_collision_velocity_filter.cpp.

Member Data Documentation

costmap_2d::Costmap2DROS* CollisionVelocityFilter::anti_collision_costmap_
private

Definition at line 125 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::atheta_max_
private

Definition at line 177 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::ax_max_
private

Definition at line 177 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::ay_max_
private

Definition at line 177 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::closest_obstacle_angle_
private

Definition at line 186 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::closest_obstacle_dist_
private

Definition at line 186 of file cob_collision_velocity_filter.h.

int CollisionVelocityFilter::costmap_obstacle_treshold_
private

Definition at line 169 of file cob_collision_velocity_filter.h.

bool CollisionVelocityFilter::costmap_received_
private

Definition at line 183 of file cob_collision_velocity_filter.h.

dynamic_reconfigure::Server<cob_collision_velocity_filter::CollisionVelocityFilterConfig> CollisionVelocityFilter::dyn_server_

dynamic reconfigure

Definition at line 119 of file cob_collision_velocity_filter.h.

dynamic_reconfigure::Server<cob_collision_velocity_filter::CollisionVelocityFilterConfig>::CallbackType CollisionVelocityFilter::dynCB_

Definition at line 120 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_front_
private

Definition at line 181 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_front_initial_
private

Definition at line 182 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_left_
private

Definition at line 181 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_left_initial_
private

Definition at line 182 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_rear_
private

Definition at line 181 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_rear_initial_
private

Definition at line 182 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_right_
private

Definition at line 181 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_right_initial_
private

Definition at line 182 of file cob_collision_velocity_filter.h.

ros::Timer CollisionVelocityFilter::get_footprint_timer_

Timer for periodically calling GetFootprint Service.

Definition at line 109 of file cob_collision_velocity_filter.h.

std::string CollisionVelocityFilter::global_frame_
private

Definition at line 172 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::influence_radius_
private

Definition at line 185 of file cob_collision_velocity_filter.h.

ros::Subscriber CollisionVelocityFilter::joystick_velocity_sub_

declaration of subscriber

Definition at line 116 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::kp_
private

Definition at line 190 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::kv_
private

Definition at line 190 of file cob_collision_velocity_filter.h.

nav_msgs::OccupancyGrid CollisionVelocityFilter::last_costmap_received_
private

Definition at line 184 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::last_time_
private

Definition at line 189 of file cob_collision_velocity_filter.h.

pthread_mutex_t CollisionVelocityFilter::m_mutex
private

Definition at line 166 of file cob_collision_velocity_filter.h.

ros::NodeHandle CollisionVelocityFilter::nh_

create a handle for this node, initialize node

Definition at line 103 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::obstacle_damping_dist_
private

Definition at line 185 of file cob_collision_velocity_filter.h.

ros::Subscriber CollisionVelocityFilter::obstacles_sub_

Definition at line 116 of file cob_collision_velocity_filter.h.

ros::NodeHandle CollisionVelocityFilter::pnh_

Definition at line 106 of file cob_collision_velocity_filter.h.

nav_msgs::OccupancyGrid CollisionVelocityFilter::relevant_obstacles_
private

Definition at line 184 of file cob_collision_velocity_filter.h.

std::vector<geometry_msgs::Point> CollisionVelocityFilter::robot_footprint_
private

Definition at line 180 of file cob_collision_velocity_filter.h.

std::string CollisionVelocityFilter::robot_frame_
private

Definition at line 172 of file cob_collision_velocity_filter.h.

geometry_msgs::Vector3 CollisionVelocityFilter::robot_twist_angular_
private

Definition at line 175 of file cob_collision_velocity_filter.h.

geometry_msgs::Vector3 CollisionVelocityFilter::robot_twist_linear_
private

Definition at line 175 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::stop_threshold_
private

Definition at line 185 of file cob_collision_velocity_filter.h.

ros::Publisher CollisionVelocityFilter::topic_pub_command_

declaration of publisher

Definition at line 112 of file cob_collision_velocity_filter.h.

ros::Publisher CollisionVelocityFilter::topic_pub_relevant_obstacles_

Definition at line 113 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::use_circumscribed_threshold_
private

Definition at line 185 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::v_max_
private

Definition at line 176 of file cob_collision_velocity_filter.h.

cob_collision_velocity_filter::VelocityLimitedMarker CollisionVelocityFilter::velocity_limited_marker_
private

Definition at line 195 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::virt_mass_
private

Definition at line 192 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::vtheta_last_
private

Definition at line 191 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::vtheta_max_
private

Definition at line 176 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::vx_last_
private

Definition at line 191 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::vy_last_
private

Definition at line 191 of file cob_collision_velocity_filter.h.

The documentation for this class was generated from the following files:

cob_collision_velocity_filter
Author(s): Matthias Gruhler, Michal Spanel
autogenerated on Thu Apr 8 2021 02:39:36