SMACC
Loading...
Searching...
No Matches
Public Member Functions | Public Attributes | Private Member Functions | Private Attributes | List of all members
cl_move_base_z::WaypointNavigator Class Reference

#include <waypoints_navigator.h>

Inheritance diagram for cl_move_base_z::WaypointNavigator:
Inheritance graph
Collaboration diagram for cl_move_base_z::WaypointNavigator:
Collaboration graph

Public Member Functions

 WaypointNavigator ()
 
virtual void onInitialize () override
 
void insertWaypoint (int index, geometry_msgs::Pose &newpose)
 
void removeWaypoint (int index)
 
void loadWayPointsFromFile (std::string filepath)
 
void setWaypoints (const std::vector< geometry_msgs::Pose > &waypoints)
 
void setWaypoints (const std::vector< Pose2D > &waypoints)
 
void sendNextGoal ()
 
const std::vector< geometry_msgs::Pose > & getWaypoints () const
 
long getCurrentWaypointIndex () const
 
template<typename TOrthogonal , typename TSourceObject >
void onOrthogonalAllocation ()
 
- Public Member Functions inherited from smacc::ISmaccComponent
 ISmaccComponent ()
 
virtual ~ISmaccComponent ()
 
virtual std::string getName () const
 

Public Attributes

WaypointEventDispatcher waypointsEventDispatcher
 
ClMoveBaseZclient_
 
int currentWaypoint_
 

Private Member Functions

void onGoalReached (ClMoveBaseZ::ResultConstPtr &res)
 

Private Attributes

std::vector< geometry_msgs::Pose > waypoints_
 
boost::signals2::connection succeddedConnection_
 

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
ISmaccStateMachinestateMachine_
 
ISmaccClientowner_
 

Detailed Description

Definition at line 32 of file waypoints_navigator.h.

Constructor & Destructor Documentation

◆ WaypointNavigator()

cl_move_base_z::WaypointNavigator::WaypointNavigator ( )

Definition at line 14 of file waypoints_navigator.cpp.

16 waypoints_(0)
17{
18}
std::vector< geometry_msgs::Pose > waypoints_

Member Function Documentation

◆ getCurrentWaypointIndex()

long cl_move_base_z::WaypointNavigator::getCurrentWaypointIndex ( ) const

Definition at line 110 of file waypoints_navigator.cpp.

111{
112 return currentWaypoint_;
113}

References currentWaypoint_.

◆ getWaypoints()

const std::vector< geometry_msgs::Pose > & cl_move_base_z::WaypointNavigator::getWaypoints ( ) const

Definition at line 105 of file waypoints_navigator.cpp.

106{
107 return waypoints_;
108}

References waypoints_.

◆ insertWaypoint()

void cl_move_base_z::WaypointNavigator::insertWaypoint ( int  index,
geometry_msgs::Pose &  newpose 
)

Definition at line 69 of file waypoints_navigator.cpp.

70{
71 if (index >= 0 && index <= waypoints_.size())
72 {
73 waypoints_.insert(waypoints_.begin(), index, newpose);
74 }
75}

References waypoints_.

◆ loadWayPointsFromFile()

void cl_move_base_z::WaypointNavigator::loadWayPointsFromFile ( std::string  filepath)

Definition at line 116 of file waypoints_navigator.cpp.

117{
118 this->waypoints_.clear();
119 std::ifstream ifs(filepath.c_str(), std::ifstream::in);
120 if (ifs.good() == false)
121 {
122 throw std::string("Waypoints file not found");
123 }
124
125 try
126 {
127
128#ifdef HAVE_NEW_YAMLCPP
129 YAML::Node node = YAML::Load(ifs);
130#else
131 YAML::Parser parser(ifs);
132 parser.GetNextDocument(node);
133#endif
134
135#ifdef HAVE_NEW_YAMLCPP
136 const YAML::Node &wp_node_tmp = node["waypoints"];
137 const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
138#else
139 const YAML::Node *wp_node = node.FindValue("waypoints");
140#endif
141
142 if (wp_node != NULL)
143 {
144 for (uint64_t i = 0; i < wp_node->size(); ++i)
145 {
146 // Parse waypoint entries on YAML
147 geometry_msgs::Pose wp;
148
149 try
150 {
151 // (*wp_node)[i]["name"] >> wp.name;
152 // (*wp_node)[i]["frame_id"] >> wp.header.frame_id;
153 wp.position.x = (*wp_node)[i]["position"]["x"].as<double>();
154 wp.position.y = (*wp_node)[i]["position"]["y"].as<double>();
155 wp.position.z = (*wp_node)[i]["position"]["z"].as<double>();
156 wp.orientation.x = (*wp_node)[i]["orientation"]["x"].as<double>();
157 wp.orientation.y = (*wp_node)[i]["orientation"]["y"].as<double>();
158 wp.orientation.z = (*wp_node)[i]["orientation"]["z"].as<double>();
159 wp.orientation.w = (*wp_node)[i]["orientation"]["w"].as<double>();
160
161 this->waypoints_.push_back(wp);
162 }
163 catch (...)
164 {
165 ROS_ERROR("parsing waypoint file, syntax error in point %ld", i);
166 }
167 }
168 ROS_INFO_STREAM("Parsed " << this->waypoints_.size() << " waypoints.");
169 }
170 else
171 {
172 ROS_WARN_STREAM("Couldn't find any waypoints in the provided yaml file.");
173 }
174 }
175 catch (const YAML::ParserException &ex)
176 {
177 ROS_ERROR_STREAM("Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what());
178 }
179}

References waypoints_.

◆ onGoalReached()

void cl_move_base_z::WaypointNavigator::onGoalReached ( ClMoveBaseZ::ResultConstPtr res)
private

Definition at line 25 of file waypoints_navigator.cpp.

References currentWaypoint_, cl_move_base_z::WaypointEventDispatcher::postWaypointEvent(), succeddedConnection_, and waypointsEventDispatcher.

Referenced by sendNextGoal().

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

◆ onInitialize()

void cl_move_base_z::WaypointNavigator::onInitialize ( )
overridevirtual

Reimplemented from smacc::ISmaccComponent.

Definition at line 20 of file waypoints_navigator.cpp.

21{
22 client_ = dynamic_cast<ClMoveBaseZ *>(owner_);
23}
ISmaccClient * owner_
Definition: component.h:57

References client_, and smacc::ISmaccComponent::owner_.

◆ onOrthogonalAllocation()

template<typename TOrthogonal , typename TSourceObject >
void cl_move_base_z::WaypointNavigator::onOrthogonalAllocation ( )
inline

Definition at line 60 of file waypoints_navigator.h.

61 {
62 waypointsEventDispatcher.initialize<TSourceObject, TOrthogonal>(client_);
63 }

References client_, cl_move_base_z::WaypointEventDispatcher::initialize(), and waypointsEventDispatcher.

Here is the call graph for this function:

◆ removeWaypoint()

void cl_move_base_z::WaypointNavigator::removeWaypoint ( int  index)

Definition at line 97 of file waypoints_navigator.cpp.

98{
99 if (index >= 0 && index < waypoints_.size())
100 {
101 waypoints_.erase(waypoints_.begin() + index);
102 }
103}

References waypoints_.

◆ sendNextGoal()

void cl_move_base_z::WaypointNavigator::sendNextGoal ( )

Definition at line 32 of file waypoints_navigator.cpp.

33{
34 if (currentWaypoint_ >= 0 && currentWaypoint_ < waypoints_.size())
35 {
36 auto &next = waypoints_[currentWaypoint_];
37
40 auto pose = p->toPoseMsg();
41
42 ClMoveBaseZ::Goal goal;
43 goal.target_pose.header.frame_id = p->getReferenceFrame();
44 goal.target_pose.header.stamp = ros::Time::now();
45 goal.target_pose.pose = next;
46
47 auto plannerSwitcher = client_->getComponent<PlannerSwitcher>();
48 plannerSwitcher->setDefaultPlanners();
49
50 ros::spinOnce();
51 ros::Duration(5).sleep();
52
53 if (odomTracker != nullptr)
54 {
55 odomTracker->pushPath("FreeNavigationToGoalWaypointPose");
56 odomTracker->setStartPoint(pose);
58 }
59
61 client_->sendGoal(goal);
62 }
63 else
64 {
65 ROS_WARN("[WaypointsNavigator] All waypoints were consumed. There is no more waypoints available.");
66 }
67}
void onGoalReached(ClMoveBaseZ::ResultConstPtr &res)
This class track the required distance of the cord based on the external localization system.
Definition: odom_tracker.h:47
TComponent * getComponent()
boost::signals2::connection onSucceeded(void(T::*callback)(ResultConstPtr &), T *object)

References client_, currentWaypoint_, smacc::ISmaccClient::getComponent(), onGoalReached(), smacc::client_bases::SmaccActionClientBase< ActionType >::onSucceeded(), cl_move_base_z::odom_tracker::RECORD_PATH, smacc::client_bases::SmaccActionClientBase< ActionType >::sendGoal(), cl_move_base_z::PlannerSwitcher::setDefaultPlanners(), succeddedConnection_, and waypoints_.

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

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

◆ setWaypoints() [1/2]

void cl_move_base_z::WaypointNavigator::setWaypoints ( const std::vector< geometry_msgs::Pose > &  waypoints)

Definition at line 77 of file waypoints_navigator.cpp.

78{
79 this->waypoints_ = waypoints;
80}

References waypoints_.

◆ setWaypoints() [2/2]

void cl_move_base_z::WaypointNavigator::setWaypoints ( const std::vector< Pose2D > &  waypoints)

Definition at line 82 of file waypoints_navigator.cpp.

83{
84 this->waypoints_.clear();
85 for (auto &p : waypoints)
86 {
87 geometry_msgs::Pose pose;
88 pose.position.x = p.x_;
89 pose.position.y = p.y_;
90 pose.position.z = 0.0;
91 pose.orientation = tf::createQuaternionMsgFromYaw(p.yaw_);
92
93 this->waypoints_.push_back(pose);
94 }
95}

References waypoints_.

Member Data Documentation

◆ client_

ClMoveBaseZ* cl_move_base_z::WaypointNavigator::client_

Definition at line 37 of file waypoints_navigator.h.

Referenced by onInitialize(), onOrthogonalAllocation(), and sendNextGoal().

◆ currentWaypoint_

int cl_move_base_z::WaypointNavigator::currentWaypoint_

Definition at line 65 of file waypoints_navigator.h.

Referenced by getCurrentWaypointIndex(), onGoalReached(), and sendNextGoal().

◆ succeddedConnection_

boost::signals2::connection cl_move_base_z::WaypointNavigator::succeddedConnection_
private

Definition at line 72 of file waypoints_navigator.h.

Referenced by onGoalReached(), and sendNextGoal().

◆ waypoints_

std::vector<geometry_msgs::Pose> cl_move_base_z::WaypointNavigator::waypoints_
private

◆ waypointsEventDispatcher

WaypointEventDispatcher cl_move_base_z::WaypointNavigator::waypointsEventDispatcher

Definition at line 35 of file waypoints_navigator.h.

Referenced by onGoalReached(), and onOrthogonalAllocation().


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