cob_collision_velocity_filter.h

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#ifndef COB_COLLISION_VELOCITY_FILTER_H
#define COB_COLLISION_VELOCITY_FILTER_H

//##################
//#### includes ####

// standard includes
//--

// ROS includes
#include <ros/ros.h>
#include <XmlRpc.h>

#include <pthread.h>

// ROS message includes
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Polygon.h>
#include <nav_msgs/OccupancyGrid.h>

#include <boost/tokenizer.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>

// ROS service includes
#include "cob_footprint_observer/GetFootprint.h"

// dynamic reconfigure includes
#include <dynamic_reconfigure/server.h>
#include <cob_collision_velocity_filter/CollisionVelocityFilterConfig.h>

// BUT velocity limited marker
#include "velocity_limited_marker.h"

// Costmap for obstacle detection
#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
class CollisionVelocityFilter
{
public:

  CollisionVelocityFilter(costmap_2d::Costmap2DROS * costmap);

  ~CollisionVelocityFilter();

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

  void readObstacles();

  void getFootprint(const ros::TimerEvent&);

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

  //public
  ros::NodeHandle nh_;

  //private
  ros::NodeHandle pnh_;

  ros::Timer get_footprint_timer_;

  ros::Publisher topic_pub_command_;
  ros::Publisher topic_pub_relevant_obstacles_;

  ros::Subscriber joystick_velocity_sub_, obstacles_sub_;

  dynamic_reconfigure::Server<cob_collision_velocity_filter::CollisionVelocityFilterConfig> dyn_server_;
  dynamic_reconfigure::Server<cob_collision_velocity_filter::CollisionVelocityFilterConfig>::CallbackType dynCB_;

private:
  /* core functions */

  costmap_2d::Costmap2DROS* anti_collision_costmap_;
  //costmap_2d::Costmap2D costmap;
  void performControllerStep();

  void obstacleHandler();

  /* helper functions */

  double sign(double x);

  double getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b);

  bool obstacleValid(double x_obstacle, double y_obstacle);

  void stopMovement();

  pthread_mutex_t m_mutex;

  //obstacle_treshold
  int costmap_obstacle_treshold_;

  //frames
  std::string global_frame_, robot_frame_;

  //velocity
  geometry_msgs::Vector3 robot_twist_linear_, robot_twist_angular_;
  double v_max_, vtheta_max_;
  double ax_max_, ay_max_, atheta_max_;

  //obstacle avoidance
  std::vector<geometry_msgs::Point> robot_footprint_;
  double footprint_left_, footprint_right_, footprint_front_, footprint_rear_;
  double footprint_left_initial_, footprint_right_initial_, footprint_front_initial_, footprint_rear_initial_;
  bool costmap_received_;
  nav_msgs::OccupancyGrid last_costmap_received_, relevant_obstacles_;
  double influence_radius_, stop_threshold_, obstacle_damping_dist_, use_circumscribed_threshold_;
  double closest_obstacle_dist_, closest_obstacle_angle_;

  // variables for slow down behavior
  double last_time_;
  double kp_, kv_;
  double vx_last_, vy_last_, vtheta_last_;
  double virt_mass_;

  // BUT velocity limited marker
  cob_collision_velocity_filter::VelocityLimitedMarker velocity_limited_marker_;

};
//CollisionVelocityFilter

#endif