cl_move_base_z::PlannerSwitcher Class Reference

#include <planner_switcher.h>

Inheritance diagram for cl_move_base_z::PlannerSwitcher:

Collaboration diagram for cl_move_base_z::PlannerSwitcher:

Public Member Functions
	PlannerSwitcher ()
void	setBackwardPlanner ()
void	setUndoPathBackwardsPlannerConfiguration ()
void	setUndoPathPureSpinningPlannerConfiguration ()
void	setForwardPlanner ()
void	setPureSpinningPlanner ()
virtual void	onInitialize () override
void	setDefaultPlanners ()
Public Member Functions inherited from smacc::ISmaccComponent
	ISmaccComponent ()
virtual	~ISmaccComponent ()
virtual std::string	getName () const

Private Member Functions
void	updatePlanners (bool subscribecallback=true)
void	dynreconfCallback (const dynamic_reconfigure::Config::ConstPtr &configuration_update)

Private Attributes
std::string	desired_global_planner_
std::string	desired_local_planner_
ros::Subscriber	dynrecofSub_
bool	set_planners_mode_flag

Additional Inherited Members
Protected Member Functions inherited from smacc::ISmaccComponent
virtual void	initialize (ISmaccClient *owner)
void	setStateMachine (ISmaccStateMachine *stateMachine)
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage)
template<typename TClient >
void	requiresClient (TClient *&requiredClientStorage)
virtual void	onInitialize ()
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingComponent (TArgs... targs)
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingNamedComponent (std::string name, TArgs... targs)
Protected Attributes inherited from smacc::ISmaccComponent
ISmaccStateMachine *	stateMachine_
ISmaccClient *	owner_

Detailed Description

Definition at line 19 of file planner_switcher.h.

Constructor & Destructor Documentation

PlannerSwitcher()

cl_move_base_z::PlannerSwitcher::PlannerSwitcher

(

)

Definition at line 12 of file planner_switcher.cpp.

{
}

Member Function Documentation

dynreconfCallback()

void cl_move_base_z::PlannerSwitcher::dynreconfCallback ( const dynamic_reconfigure::Config::ConstPtr &  configuration_update )

private

Definition at line 118 of file planner_switcher.cpp.

{
  auto gp = std::find_if(configuration_update->strs.begin(), configuration_update->strs.begin(),
                         [&](const dynamic_reconfigure::StrParameter &p) {
                           return p.name == "base_global_planner" && p.value == desired_global_planner_;
                         });
 
  auto lp = std::find_if(configuration_update->strs.begin(), configuration_update->strs.begin(),
                         [&](const dynamic_reconfigure::StrParameter &p) {
                           return p.name == "base_local_planner" && p.value == desired_local_planner_;
                         });
 
  if (gp == configuration_update->strs.end() || lp == configuration_update->strs.end())
  {
    ROS_INFO("[PlannerSwitcher] After planner update it is noticed an incorrect move_base planner configuration. Resending request.");
    set_planners_mode_flag = false;
    updatePlanners(false);
  }
  else
  {
    ROS_INFO("[PlannerSwitcher] Planners correctly configured according to parameter update callback");
    set_planners_mode_flag = true;
  }
}

References set_planners_mode_flag, and updatePlanners().

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

onInitialize()

void cl_move_base_z::PlannerSwitcher::onInitialize ( )

overridevirtual

Reimplemented from smacc::ISmaccComponent.

Definition at line 16 of file planner_switcher.cpp.

{
  auto client_ = dynamic_cast<ClMoveBaseZ *>(owner_);
  ros::NodeHandle nh(client_->name_);
  dynrecofSub_ = nh.subscribe<dynamic_reconfigure::Config>("/move_base/parameter_updates", 1, boost::bind(&PlannerSwitcher::dynreconfCallback, this, _1));
}

References dynrecofSub_, dynreconfCallback(), and smacc::ISmaccComponent::owner_.

Here is the call graph for this function:

setBackwardPlanner()

void cl_move_base_z::PlannerSwitcher::setBackwardPlanner

(

)

Definition at line 39 of file planner_switcher.cpp.

{
  ROS_INFO("[PlannerSwitcher] Planner Switcher: Trying to set BackwardPlanner");
  desired_global_planner_ = "backward_global_planner/BackwardGlobalPlanner";
  desired_local_planner_ = "backward_local_planner/BackwardLocalPlanner";
  updatePlanners();
}

References desired_global_planner_, desired_local_planner_, and updatePlanners().

Referenced by cl_move_base_z::CbNavigateBackwards::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

setDefaultPlanners()

void cl_move_base_z::PlannerSwitcher::setDefaultPlanners

(

)

Definition at line 64 of file planner_switcher.cpp.

{
  desired_global_planner_ = "navfn/NavfnROS";
  desired_local_planner_ = "base_local_planner/TrajectoryPlannerROS";
  updatePlanners();
}

References desired_global_planner_, desired_local_planner_, and updatePlanners().

Referenced by cl_move_base_z::CbNavigateGlobalPosition::onEntry(), cl_move_base_z::CbRotate::onEntry(), and cl_move_base_z::WaypointNavigator::sendNextGoal().

Here is the call graph for this function:

Here is the caller graph for this function:

setForwardPlanner()

void cl_move_base_z::PlannerSwitcher::setForwardPlanner

(

)

Definition at line 47 of file planner_switcher.cpp.

{
  ROS_INFO("[PlannerSwitcher] Planner Switcher: Trying to set ForwardPlanner");
  desired_global_planner_ = "forward_global_planner/ForwardGlobalPlanner";
  desired_local_planner_ = "forward_local_planner/ForwardLocalPlanner";
  updatePlanners();
}

References desired_global_planner_, desired_local_planner_, and updatePlanners().

Referenced by cl_move_base_z::CbNavigateForward::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

setPureSpinningPlanner()

void cl_move_base_z::PlannerSwitcher::setPureSpinningPlanner

(

)

Definition at line 56 of file planner_switcher.cpp.

{
  ROS_INFO("[PlannerSwitcher] Planner Switcher: Trying to set PureSpinningPlanner");
  desired_global_planner_ = "forward_global_planner/ForwardGlobalPlanner";
  desired_local_planner_ = "pure_spinning_local_planner/PureSpinningLocalPlanner";
  updatePlanners();
}

References desired_global_planner_, desired_local_planner_, and updatePlanners().

Referenced by cl_move_base_z::CbAbsoluteRotate::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

setUndoPathBackwardsPlannerConfiguration()

void cl_move_base_z::PlannerSwitcher::setUndoPathBackwardsPlannerConfiguration

(

)

Definition at line 23 of file planner_switcher.cpp.

{
  ROS_INFO("[PlannerSwitcher] Planner Switcher: Trying to set BackwardPlanner");
  desired_global_planner_ = "undo_path_global_planner/UndoPathGlobalPlanner";
  desired_local_planner_ = "backward_local_planner/BackwardLocalPlanner";
  updatePlanners();
}

References desired_global_planner_, desired_local_planner_, and updatePlanners().

Here is the call graph for this function:

setUndoPathPureSpinningPlannerConfiguration()

void cl_move_base_z::PlannerSwitcher::setUndoPathPureSpinningPlannerConfiguration

(

)

Definition at line 31 of file planner_switcher.cpp.

{
  ROS_INFO("[PlannerSwitcher] Planner Switcher: Trying to set BackwardPlanner");
  desired_global_planner_ = "undo_path_global_planner/UndoPathGlobalPlanner";
  desired_local_planner_ = "pure_spinning_local_planner/PureSpinningLocalPlanner";
  updatePlanners();
}

References desired_global_planner_, desired_local_planner_, and updatePlanners().

Here is the call graph for this function:

updatePlanners()

void cl_move_base_z::PlannerSwitcher::updatePlanners ( bool  subscribecallback = true )

private

Definition at line 71 of file planner_switcher.cpp.

{
  ROS_INFO_STREAM("[PlannerSwitcher] Setting global planner: " << desired_global_planner_);
  ROS_INFO_STREAM("[PlannerSwitcher] Setting local planner: " << desired_local_planner_);
 
  dynamic_reconfigure::ReconfigureRequest srv_req;
  dynamic_reconfigure::ReconfigureResponse srv_resp;
  dynamic_reconfigure::StrParameter local_planner, global_planner;
  dynamic_reconfigure::Config conf;
 
  local_planner.name = "base_local_planner";
  local_planner.value = desired_local_planner_;
  conf.strs.push_back(local_planner);
 
  global_planner.name = "base_global_planner";
  global_planner.value = desired_global_planner_;
  conf.strs.push_back(global_planner);
 
  srv_req.config = conf;
  ROS_INFO("setting values of the dynamic reconfigure server");
  bool error;
  do
  {
    bool res = ros::service::call("/move_base/set_parameters", srv_req, srv_resp);
    ros::spinOnce();
    ros::Duration(0.5).sleep();
 
    error = false;
    auto baselocalPlannerIt = std::find_if(srv_resp.config.strs.begin(), srv_resp.config.strs.end(), [](auto sp){return sp.name == "base_local_planner";});
    auto baseglobalPlannerIt = std::find_if(srv_resp.config.strs.begin(), srv_resp.config.strs.end(), [](auto sp){return sp.name == "base_global_planner";});
 
    auto updatedLocalPlanner = baselocalPlannerIt->value;
    auto updatedGlobalPlanner = baseglobalPlannerIt->value;
    ROS_INFO_STREAM("[PlannerSwitcher] Selected base local planner: " << updatedLocalPlanner);
    ROS_INFO_STREAM("[PlannerSwitcher] Selected base global planner: " << updatedGlobalPlanner);
 
    if(updatedGlobalPlanner != desired_global_planner_ || updatedLocalPlanner!= desired_local_planner_)
    {
      ROS_WARN("[PlannerSwitcher] Planners not correctly configured. Waiting 1 second and retrying...");
      ROS_INFO_STREAM("[PlannerSwitcher] Response: " << srv_resp);
      error = true;
    }
 
  }while(error);
 
}

References desired_global_planner_, and desired_local_planner_.

Referenced by dynreconfCallback(), setBackwardPlanner(), setDefaultPlanners(), setForwardPlanner(), setPureSpinningPlanner(), setUndoPathBackwardsPlannerConfiguration(), and setUndoPathPureSpinningPlannerConfiguration().

Here is the caller graph for this function:

Member Data Documentation

desired_global_planner_

std::string cl_move_base_z::PlannerSwitcher::desired_global_planner_

private

Definition at line 36 of file planner_switcher.h.

Referenced by setBackwardPlanner(), setDefaultPlanners(), setForwardPlanner(), setPureSpinningPlanner(), setUndoPathBackwardsPlannerConfiguration(), setUndoPathPureSpinningPlannerConfiguration(), and updatePlanners().

desired_local_planner_

std::string cl_move_base_z::PlannerSwitcher::desired_local_planner_

private

Definition at line 37 of file planner_switcher.h.

Referenced by setBackwardPlanner(), setDefaultPlanners(), setForwardPlanner(), setPureSpinningPlanner(), setUndoPathBackwardsPlannerConfiguration(), setUndoPathPureSpinningPlannerConfiguration(), and updatePlanners().

dynrecofSub_

ros::Subscriber cl_move_base_z::PlannerSwitcher::dynrecofSub_

private

Definition at line 38 of file planner_switcher.h.

Referenced by onInitialize().

set_planners_mode_flag

bool cl_move_base_z::PlannerSwitcher::set_planners_mode_flag

private

Definition at line 39 of file planner_switcher.h.

Referenced by dynreconfCallback().

---

The documentation for this class was generated from the following files:

• smacc_client_library/move_base_z_client/move_base_z_client_plugin/include/move_base_z_client_plugin/components/planner_switcher/planner_switcher.h
• smacc_client_library/move_base_z_client/move_base_z_client_plugin/src/components/planner_switcher/planner_switcher.cpp