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

#include <velocity_limited_marker.h>

Public Member Functions
	CollisionVelocityFilter ()
	Constructor.
void	dynamicReconfigureCB (const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config, const uint32_t level)
	Dynamic reconfigure callback.
void	getFootprintServiceCB (const ros::TimerEvent &)
	Timer callback, calls GetFootprint Service and adjusts footprint.
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)
void	obstaclesCB (const nav_msgs::GridCells::ConstPtr &obstacles)
	reads obstacles from costmap
	~CollisionVelocityFilter ()
	Destructor.
Public Attributes
dynamic_reconfigure::Server < cob_collision_velocity_filter::CollisionVelocityFilterConfig >	dyn_server_
	dynamic reconfigure
dynamic_reconfigure::Server < cob_collision_velocity_filter::CollisionVelocityFilterConfig > ::CallbackType	dynCB_
ros::Timer	get_footprint_timer_
	Timer for periodically calling GetFootprint Service.
ros::Subscriber	joystick_velocity_sub_
	declaration of subscriber
ros::NodeHandle	nh_
	create a handle for this node, initialize node
ros::Subscriber	obstacles_sub_
ros::ServiceClient	srv_client_get_footprint_
	declaration of service client
ros::Publisher	topic_pub_command_
	declaration of publisher
ros::Publisher	topic_pub_relevant_obstacles_
Private Member Functions
double	getDistance2d (geometry_msgs::Point a, geometry_msgs::Point b)
	computes distance between two points
std::vector< geometry_msgs::Point >	loadRobotFootprint (ros::NodeHandle node)
	loads the robot footprint published by the local costmap
void	obstacleHandler ()
	checks for obstacles in driving direction of the robot (rotation included) and publishes relevant obstacles
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
void	performControllerStep ()
	checks distance to obstacles in driving direction and slows down/stops robot and publishes command velocity to robot
double	sign (double x)
	returns the sign of x
void	stopMovement ()
	stops movement of the robot
Private Attributes
double	atheta_max_
double	ax_max_
double	ay_max_
double	closest_obstacle_angle_
double	closest_obstacle_dist_
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::GridCells	last_costmap_received_
double	last_time_
pthread_mutex_t	m_mutex
double	obstacle_damping_dist_
nav_msgs::GridCells	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 ( )

Constructor.

Definition at line 55 of file cob_collision_velocity_filter.cpp.

CollisionVelocityFilter::~CollisionVelocityFilter ( )

Destructor.

Definition at line 159 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 233 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 663 of file cob_collision_velocity_filter.cpp.

void CollisionVelocityFilter::getFootprintServiceCB ( const ros::TimerEvent & )

Timer callback, calls GetFootprint Service and adjusts footprint.

Definition at line 191 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 162 of file cob_collision_velocity_filter.cpp.

std::vector< geometry_msgs::Point > CollisionVelocityFilter::loadRobotFootprint ( ros::NodeHandle node ) [private]

loads the robot footprint published by the local costmap

Parameters:

node	- NodeHandle to the local costmap

Returns:: footprint polygon as vector

Definition at line 519 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 365 of file cob_collision_velocity_filter.cpp.

void CollisionVelocityFilter::obstaclesCB ( const nav_msgs::GridCells::ConstPtr & obstacles )

reads obstacles from costmap

Parameters:

obstacles - 2D occupancy grid in rolling window mode!

Definition at line 178 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 672 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 258 of file cob_collision_velocity_filter.cpp.

double CollisionVelocityFilter::sign ( double x ) [private]

returns the sign of x

Definition at line 667 of file cob_collision_velocity_filter.cpp.

void CollisionVelocityFilter::stopMovement ( ) [private]

stops movement of the robot

Definition at line 681 of file cob_collision_velocity_filter.cpp.

Member Data Documentation

double CollisionVelocityFilter::atheta_max_ [private]

Definition at line 212 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::ax_max_ [private]

Definition at line 212 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::ay_max_ [private]

Definition at line 212 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::closest_obstacle_angle_ [private]

Definition at line 221 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::closest_obstacle_dist_ [private]

Definition at line 221 of file cob_collision_velocity_filter.h.

bool CollisionVelocityFilter::costmap_received_ [private]

Definition at line 218 of file cob_collision_velocity_filter.h.

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

dynamic reconfigure

Definition at line 150 of file cob_collision_velocity_filter.h.

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

Definition at line 151 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_front_ [private]

Definition at line 216 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_front_initial_ [private]

Definition at line 217 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_left_ [private]

Definition at line 216 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_left_initial_ [private]

Definition at line 217 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_rear_ [private]

Definition at line 216 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_rear_initial_ [private]

Definition at line 217 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_right_ [private]

Definition at line 216 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::footprint_right_initial_ [private]

Definition at line 217 of file cob_collision_velocity_filter.h.

ros::Timer CollisionVelocityFilter::get_footprint_timer_

Timer for periodically calling GetFootprint Service.

Definition at line 137 of file cob_collision_velocity_filter.h.

std::string CollisionVelocityFilter::global_frame_ [private]

Definition at line 207 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::influence_radius_ [private]

Definition at line 220 of file cob_collision_velocity_filter.h.

ros::Subscriber CollisionVelocityFilter::joystick_velocity_sub_

declaration of subscriber

Definition at line 144 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::kp_ [private]

Definition at line 225 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::kv_ [private]

Definition at line 225 of file cob_collision_velocity_filter.h.

nav_msgs::GridCells CollisionVelocityFilter::last_costmap_received_ [private]

Definition at line 219 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::last_time_ [private]

Definition at line 224 of file cob_collision_velocity_filter.h.

pthread_mutex_t CollisionVelocityFilter::m_mutex [private]

Definition at line 204 of file cob_collision_velocity_filter.h.

ros::NodeHandle CollisionVelocityFilter::nh_

create a handle for this node, initialize node

Definition at line 134 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::obstacle_damping_dist_ [private]

Definition at line 220 of file cob_collision_velocity_filter.h.

ros::Subscriber CollisionVelocityFilter::obstacles_sub_

Definition at line 144 of file cob_collision_velocity_filter.h.

nav_msgs::GridCells CollisionVelocityFilter::relevant_obstacles_ [private]

Definition at line 219 of file cob_collision_velocity_filter.h.

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

Definition at line 215 of file cob_collision_velocity_filter.h.

std::string CollisionVelocityFilter::robot_frame_ [private]

Definition at line 207 of file cob_collision_velocity_filter.h.

geometry_msgs::Vector3 CollisionVelocityFilter::robot_twist_angular_ [private]

Definition at line 210 of file cob_collision_velocity_filter.h.

geometry_msgs::Vector3 CollisionVelocityFilter::robot_twist_linear_ [private]

Definition at line 210 of file cob_collision_velocity_filter.h.

ros::ServiceClient CollisionVelocityFilter::srv_client_get_footprint_

declaration of service client

Definition at line 147 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::stop_threshold_ [private]

Definition at line 220 of file cob_collision_velocity_filter.h.

ros::Publisher CollisionVelocityFilter::topic_pub_command_

declaration of publisher

Definition at line 140 of file cob_collision_velocity_filter.h.

ros::Publisher CollisionVelocityFilter::topic_pub_relevant_obstacles_

Definition at line 141 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::use_circumscribed_threshold_ [private]

Definition at line 220 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::v_max_ [private]

Definition at line 211 of file cob_collision_velocity_filter.h.

cob_collision_velocity_filter::VelocityLimitedMarker CollisionVelocityFilter::velocity_limited_marker_ [private]

Definition at line 230 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::virt_mass_ [private]

Definition at line 227 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::vtheta_last_ [private]

Definition at line 226 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::vtheta_max_ [private]

Definition at line 211 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::vx_last_ [private]

Definition at line 226 of file cob_collision_velocity_filter.h.

double CollisionVelocityFilter::vy_last_ [private]

Definition at line 226 of file cob_collision_velocity_filter.h.

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

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation