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. | |
void | dynamicReconfigureCB (const cob_collision_velocity_filter::CollisionVelocityFilterConfig &config, const uint32_t level) |
Dynamic reconfigure callback. | |
void | getFootprint (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 | readObstacles () |
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::NodeHandle | pnh_ |
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 | |
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 | |
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_ |
checks for obstacles in driving direction and stops the robot
CollisionVelocityFilter::CollisionVelocityFilter | ( | costmap_2d::Costmap2DROS * | costmap | ) |
Constructor.
Definition at line 23 of file cob_collision_velocity_filter.cpp.
CollisionVelocityFilter::~CollisionVelocityFilter | ( | ) |
Destructor.
Definition at line 140 of file cob_collision_velocity_filter.cpp.
void CollisionVelocityFilter::dynamicReconfigureCB | ( | const cob_collision_velocity_filter::CollisionVelocityFilterConfig & | config, |
const uint32_t | level | ||
) |
Dynamic reconfigure callback.
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 197 of file cob_collision_velocity_filter.cpp.
double CollisionVelocityFilter::getDistance2d | ( | geometry_msgs::Point | a, |
geometry_msgs::Point | b | ||
) | [private] |
computes distance between two points
a,b | - Points |
Definition at line 559 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 166 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)
twist | - velocity command sent as twist message (twist.linear.x/y/z, twist.angular.x/y/z) |
Definition at line 147 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 347 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
x_obstacle | - x coordinate of obstacle in occupancy grid local costmap |
y_obstacle | - y coordinate of obstacle in occupancy grid local costmap |
Definition at line 572 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 225 of file cob_collision_velocity_filter.cpp.
void CollisionVelocityFilter::readObstacles | ( | ) |
reads obstacles from costmap
obstacles | - 2D occupancy grid in rolling window mode! |
double CollisionVelocityFilter::sign | ( | double | x | ) | [private] |
returns the sign of x
Definition at line 564 of file cob_collision_velocity_filter.cpp.
void CollisionVelocityFilter::stopMovement | ( | ) | [private] |
stops movement of the robot
Definition at line 584 of file cob_collision_velocity_filter.cpp.
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.