SMACC2
Loading...
Searching...
No Matches
Public Member Functions | Private Attributes | List of all members
cl_nav2z::CpPlannerSwitcher Class Reference

#include <cp_planner_switcher.hpp>

Inheritance diagram for cl_nav2z::CpPlannerSwitcher:
Inheritance graph
Collaboration diagram for cl_nav2z::CpPlannerSwitcher:
Collaboration graph

Public Member Functions

 CpPlannerSwitcher ()
 
void onInitialize () override
 
void setDesiredGlobalPlanner (std::string)
 
void setDesiredController (std::string)
 
void commitPublish ()
 
void setBackwardPlanner (bool commit=true)
 
void setUndoPathBackwardPlanner (bool commit=true)
 
void setForwardPlanner (bool commit=true)
 
void setPureSpinningPlanner (bool commit=true)
 
void setDefaultPlanners (bool commit=true)
 
- Public Member Functions inherited from smacc2::ISmaccComponent
 ISmaccComponent ()
 
virtual ~ISmaccComponent ()
 
virtual std::string getName () const
 

Private Attributes

std::string desired_planner_
 
std::string desired_controller_
 
bool set_planners_mode_flag_
 
rclcpp::Publisher< std_msgs::msg::String >::SharedPtr planner_selector_pub_
 
rclcpp::Publisher< std_msgs::msg::String >::SharedPtr controller_selector_pub_
 
rclcpp::Publisher< std_msgs::msg::String >::SharedPtr goal_checker_selector_pub_
 

Additional Inherited Members

- Protected Member Functions inherited from smacc2::ISmaccComponent
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, bool throwExceptionIfNotExist=false)
 
template<typename TComponent >
void requiresComponent (std::string name, TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist=false)
 
template<typename TClient >
void requiresClient (TClient *&requiredClientStorage)
 
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentTypecreateSiblingComponent (TArgs... targs)
 
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentTypecreateSiblingNamedComponent (std::string name, TArgs... targs)
 
rclcpp::Node::SharedPtr getNode ()
 
rclcpp::Logger getLogger () const
 
ISmaccStateMachinegetStateMachine ()
 
- Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachinestateMachine_
 
ISmaccClientowner_
 

Detailed Description

Definition at line 31 of file cp_planner_switcher.hpp.

Constructor & Destructor Documentation

◆ CpPlannerSwitcher()

cl_nav2z::CpPlannerSwitcher::CpPlannerSwitcher ( )

Definition at line 27 of file cp_planner_switcher.cpp.

27{}

Member Function Documentation

◆ commitPublish()

void cl_nav2z::CpPlannerSwitcher::commitPublish ( )

Definition at line 99 of file cp_planner_switcher.cpp.

100{
101 std_msgs::msg::String planner_msg;
102 planner_msg.data = desired_planner_;
103 this->planner_selector_pub_->publish(planner_msg);
104
105 std_msgs::msg::String controller_msg;
106 controller_msg.data = desired_controller_;
107 this->controller_selector_pub_->publish(controller_msg);
108}
rclcpp::Publisher< std_msgs::msg::String >::SharedPtr planner_selector_pub_
rclcpp::Publisher< std_msgs::msg::String >::SharedPtr controller_selector_pub_

References controller_selector_pub_, desired_controller_, desired_planner_, and planner_selector_pub_.

Referenced by setBackwardPlanner(), setDefaultPlanners(), setForwardPlanner(), setPureSpinningPlanner(), and setUndoPathBackwardPlanner().

Here is the caller graph for this function:

◆ onInitialize()

void cl_nav2z::CpPlannerSwitcher::onInitialize ( )
overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 29 of file cp_planner_switcher.cpp.

30{
31 rclcpp::QoS qos(rclcpp::KeepLast(1));
32 qos.transient_local().reliable();
33
35 getNode()->create_publisher<std_msgs::msg::String>("planner_selector", qos);
37 getNode()->create_publisher<std_msgs::msg::String>("controller_selector", qos);
38}
rclcpp::Node::SharedPtr getNode()

References controller_selector_pub_, smacc2::ISmaccComponent::getNode(), and planner_selector_pub_.

Here is the call graph for this function:

◆ setBackwardPlanner()

void cl_nav2z::CpPlannerSwitcher::setBackwardPlanner ( bool  commit = true)

Definition at line 60 of file cp_planner_switcher.cpp.

61{
62 RCLCPP_INFO(getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set BackwardPlanner");
63
64 desired_planner_ = "BackwardGlobalPlanner";
65 desired_controller_ = "BackwardLocalPlanner";
66
67 if (commit) commitPublish();
68}
rclcpp::Logger getLogger() const

References commitPublish(), desired_controller_, desired_planner_, and smacc2::ISmaccComponent::getLogger().

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

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setDefaultPlanners()

void cl_nav2z::CpPlannerSwitcher::setDefaultPlanners ( bool  commit = true)

Definition at line 91 of file cp_planner_switcher.cpp.

92{
93 desired_planner_ = "GridBased";
94 desired_controller_ = "FollowPath";
95
96 if (commit) commitPublish();
97}

References commitPublish(), desired_controller_, and desired_planner_.

Referenced by cl_nav2z::CpWaypointNavigator::sendNextGoal().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setDesiredController()

void cl_nav2z::CpPlannerSwitcher::setDesiredController ( std::string  controllerName)

Definition at line 45 of file cp_planner_switcher.cpp.

46{
47 desired_controller_ = controllerName;
48}

References desired_controller_.

◆ setDesiredGlobalPlanner()

void cl_nav2z::CpPlannerSwitcher::setDesiredGlobalPlanner ( std::string  plannerName)

Definition at line 40 of file cp_planner_switcher.cpp.

41{
42 desired_planner_ = plannerName;
43}

References desired_planner_.

◆ setForwardPlanner()

void cl_nav2z::CpPlannerSwitcher::setForwardPlanner ( bool  commit = true)

Definition at line 70 of file cp_planner_switcher.cpp.

71{
72 RCLCPP_INFO(getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set ForwardPlanner");
73
74 desired_planner_ = "ForwardGlobalPlanner";
75 desired_controller_ = "ForwardLocalPlanner";
76
77 if (commit) commitPublish();
78}

References commitPublish(), desired_controller_, desired_planner_, and smacc2::ISmaccComponent::getLogger().

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

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setPureSpinningPlanner()

void cl_nav2z::CpPlannerSwitcher::setPureSpinningPlanner ( bool  commit = true)

Definition at line 80 of file cp_planner_switcher.cpp.

81{
82 RCLCPP_INFO(
83 getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set PureSpinningPlanner");
84
85 desired_planner_ = "ForwardGlobalPlanner";
86 desired_controller_ = "PureSpinningLocalPlanner";
87
88 if (commit) commitPublish();
89}

References commitPublish(), desired_controller_, desired_planner_, and smacc2::ISmaccComponent::getLogger().

Referenced by cl_nav2z::CbAbsoluteRotate::onEntry(), and cl_nav2z::CbRotate::onEntry().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setUndoPathBackwardPlanner()

void cl_nav2z::CpPlannerSwitcher::setUndoPathBackwardPlanner ( bool  commit = true)

Definition at line 50 of file cp_planner_switcher.cpp.

51{
52 RCLCPP_INFO(getLogger(), "[CpPlannerSwitcher] Planner Switcher: Trying to set BackwardPlanner");
53
54 desired_planner_ = "UndoPathGlobalPlanner";
55 desired_controller_ = "BackwardLocalPlanner";
56
57 if (commit) commitPublish();
58}

References commitPublish(), desired_controller_, desired_planner_, and smacc2::ISmaccComponent::getLogger().

Here is the call graph for this function:

Member Data Documentation

◆ controller_selector_pub_

rclcpp::Publisher<std_msgs::msg::String>::SharedPtr cl_nav2z::CpPlannerSwitcher::controller_selector_pub_
private

Definition at line 66 of file cp_planner_switcher.hpp.

Referenced by commitPublish(), and onInitialize().

◆ desired_controller_

std::string cl_nav2z::CpPlannerSwitcher::desired_controller_
private

◆ desired_planner_

std::string cl_nav2z::CpPlannerSwitcher::desired_planner_
private

◆ goal_checker_selector_pub_

rclcpp::Publisher<std_msgs::msg::String>::SharedPtr cl_nav2z::CpPlannerSwitcher::goal_checker_selector_pub_
private

Definition at line 68 of file cp_planner_switcher.hpp.

◆ planner_selector_pub_

rclcpp::Publisher<std_msgs::msg::String>::SharedPtr cl_nav2z::CpPlannerSwitcher::planner_selector_pub_
private

Definition at line 64 of file cp_planner_switcher.hpp.

Referenced by commitPublish(), and onInitialize().

◆ set_planners_mode_flag_

bool cl_nav2z::CpPlannerSwitcher::set_planners_mode_flag_
private

Definition at line 62 of file cp_planner_switcher.hpp.


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