Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 #ifndef MBF_COSTMAP_NAV__COSTMAP_CONTROLLER_EXECUTION_H_
00042 #define MBF_COSTMAP_NAV__COSTMAP_CONTROLLER_EXECUTION_H_
00043
00044 #include <costmap_2d/costmap_2d_ros.h>
00045 #include <mbf_costmap_nav/MoveBaseFlexConfig.h>
00046 #include <mbf_costmap_core/costmap_controller.h>
00047 #include <mbf_abstract_nav/abstract_controller_execution.h>
00048
00049 namespace mbf_costmap_nav
00050 {
00058 class CostmapControllerExecution : public mbf_abstract_nav::AbstractControllerExecution
00059 {
00060 public:
00061
00062 typedef boost::shared_ptr<costmap_2d::Costmap2DROS> CostmapPtr;
00063
00070 CostmapControllerExecution(
00071 const std::string name,
00072 const mbf_costmap_core::CostmapController::Ptr &controller_ptr,
00073 const ros::Publisher& vel_pub,
00074 const ros::Publisher& goal_pub,
00075 const TFPtr &tf_listener_ptr,
00076 CostmapPtr &costmap_ptr,
00077 const MoveBaseFlexConfig &config,
00078 boost::function<void()> setup_fn,
00079 boost::function<void()> cleanup_fn);
00080
00084 virtual ~CostmapControllerExecution();
00085
00086 protected:
00087
00097 virtual uint32_t computeVelocityCmd(
00098 const geometry_msgs::PoseStamped& robot_pose,
00099 const geometry_msgs::TwistStamped& robot_velocity,
00100 geometry_msgs::TwistStamped& vel_cmd,
00101 std::string& message);
00102
00103 private:
00104
00105 mbf_abstract_nav::MoveBaseFlexConfig toAbstract(const MoveBaseFlexConfig &config);
00106
00108 CostmapPtr &costmap_ptr_;
00109
00111 bool lock_costmap_;
00112
00114 std::string controller_name_;
00115 };
00116
00117 }
00118
00119 #endif