46 const std::string &controller_name,
50 const TFPtr &tf_listener_ptr,
52 const MoveBaseFlexConfig &config)
53 : AbstractControllerExecution(controller_name, controller_ptr, vel_pub, goal_pub,
54 tf_listener_ptr, toAbstract(config)),
55 costmap_ptr_(costmap_ptr)
68 mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
69 abstract_config.controller_frequency = config.controller_frequency;
70 abstract_config.controller_patience = config.controller_patience;
71 abstract_config.controller_max_retries = config.controller_max_retries;
72 abstract_config.oscillation_timeout = config.oscillation_timeout;
73 abstract_config.oscillation_distance = config.oscillation_distance;
74 return abstract_config;
78 const geometry_msgs::PoseStamped &robot_pose,
79 const geometry_msgs::TwistStamped &robot_velocity,
80 geometry_msgs::TwistStamped &vel_cmd,
86 boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(
costmap_ptr_->getCostmap()->getMutex()));
87 return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message);
89 return controller_->computeVelocityCommands(robot_pose, robot_velocity, vel_cmd, message);
97 ROS_WARN(
"Sensor data is out of date, we're not going to allow commanding of the base for safety");
mbf_abstract_nav::MoveBaseFlexConfig toAbstract(const MoveBaseFlexConfig &config)
const CostmapWrapper::Ptr & costmap_ptr_
Shared pointer to thr local costmap.
CostmapControllerExecution(const std::string &controller_name, const mbf_costmap_core::CostmapController::Ptr &controller_ptr, const ros::Publisher &vel_pub, const ros::Publisher &goal_pub, const TFPtr &tf_listener_ptr, const CostmapWrapper::Ptr &costmap_ptr, const MoveBaseFlexConfig &config)
Constructor.
virtual ~CostmapControllerExecution()
Destructor.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
mbf_abstract_core::AbstractController::Ptr controller_
bool safetyCheck()
Implementation-specific safety check, called during execution to ensure it's safe to drive...
bool lock_costmap_
Whether to lock costmap before calling the controller (see issue #4 for details)
virtual uint32_t computeVelocityCmd(const geometry_msgs::PoseStamped &robot_pose, const geometry_msgs::TwistStamped &robot_velocity, geometry_msgs::TwistStamped &vel_cmd, std::string &message)
Request plugin for a new velocity command. We override this method so we can lock the local costmap b...