jockey.h
Go to the documentation of this file.
00001 /*
00002  * Obstacle avoidance with OccupancyGrid (local map).
00003  *
00004  * The local map has a fixed position relative to the robot but its
00005  * orientation is constant in the world reference frame.
00006  *
00007  * Drive the robot while avoiding obstacles:
00008  * - onTraverse and onContinue: go more or less forward depending
00009  *     on obstacle position. The action never stops by itself.
00010  *
00011  * Interaction with the map (created by this jockey):
00012  * - none.
00013  *
00014  * Interaction with the map (created by other jockeys):
00015  * - none.
00016  *
00017  * Subscribers (other than map-related):
00018  * - message type, topic default name, description
00019  * - nav_msgs/OccupancyGrid, "~local_map", local map with
00020  *   fixed position relative to the robot and fixed orientation relative
00021  *   to the world reference frame.
00022  *
00023  * Publishers (other than map-related):
00024  * - message type, topic default name, description
00025  * - geometry_msgs/Twist, "~cmd_vel", set velocity.
00026  *
00027  * Services used (other than map-related):
00028  * - none.
00029  *
00030  * Parameters:
00031  * - ~robot_radius, Float, NO_DEFAULT, robot radius (m).
00032  * - ~min_distance, Float, 2 * robot_radius, if an obstacle is closer than this,
00033  *     turn and don't go forward (m).
00034  * - ~long_distance, Float, 5 * robot_radius, if no obstacle within this
00035  *     distance, go straight (m).
00036  * - ~turnrate_collide, Float, 0.4, turn rate when obstacle closer than
00037  *     min_distance_ (rad/s).
00038  * - ~max_vel, Float, 1.0, linear velocity without obstacle (m/s).
00039  * - ~vel_close_obstacle, Float, 0.5, linear velocity if obstacle between
00040  *     min_distance and long_distance (m/s).
00041  * - ~turnrate_factor, Float, 0.9, if obstacle closer than long_distance
00042  *     turnrate = -turnrate_factor_ * mean(lateral_position_of_obstacle)
00043  *     (rad.m^-1.s^-1).
00044  * - ~laser_frame, String, "base_laser_link", frame_id of the LaserScan
00045  *     messages that are used to build the local map.
00046  * - ~fake_laser_beam_count, Int, 40, beam count for the scan obtained from the
00047  *     map.
00048  * - ~range_max, Float, 5, max. range for the fake laser beams (m).
00049  */
00050 
00051 #ifndef NJ_OA_COSTMAP_JOCKEY_H
00052 #define NJ_OA_COSTMAP_JOCKEY_H
00053 
00054 #include <string>
00055 
00056 #include <geometry_msgs/Twist.h>
00057 #include <nav_msgs/OccupancyGrid.h>
00058 
00059 #include <lama_jockeys/navigating_jockey.h>
00060 #include <nj_oa_laser/jockey.h>
00061 
00062 #include <nj_oa_costmap/twist_handler.h>
00063 
00064 namespace nj_oa_costmap
00065 {
00066 
00067 class Jockey : public nj_oa_laser::Jockey
00068 {
00069   public :
00070 
00071     Jockey(const std::string& name, const double robot_radius);
00072 
00073     virtual void onTraverse();
00074 
00075   protected :
00076 
00077     void initTwistHandlerParam(TwistHandler& twist_handler);
00078 
00079   private :
00080 
00081     void handleMap(const nav_msgs::OccupancyGridConstPtr& msg);
00082 
00083     // Internals.
00084     TwistHandler twist_handler_;
00085 };
00086 
00087 } // namespace nj_oa_costmap
00088 
00089 #endif // NJ_OA_COSTMAP_JOCKEY_H


nj_oa_costmap
Author(s): Gaël Ecorchard , Karel Košnar
autogenerated on Sat Jun 8 2019 20:58:44