SMACC2
Namespaces | Classes | Typedefs | Enumerations | Functions
cl_nav2z Namespace Reference

Namespaces

namespace  backward_global_planner
 
namespace  backward_local_planner
 
namespace  forward_global_planner
 
namespace  forward_local_planner
 
namespace  odom_tracker
 
namespace  pure_spinning_local_planner
 
namespace  undo_path_global_planner
 

Classes

class  Amcl
 
class  CbAbsoluteRotate
 
class  CbNav2ZClientBehaviorBase
 
class  CbNavigateBackwards
 
class  CbNavigateForward
 
class  CbNavigateGlobalPosition
 
class  CbNavigateNextWaypoint
 
class  CbPauseSlam
 
class  CbResumeSlam
 
class  CbRetry
 
class  CbRotate
 
class  CbUndoPathBackwards
 
class  CbWaitNav2Nodes
 
class  CbWaitPose
 
class  CbWaitTransform
 
class  ClNav2Z
 
class  CostmapProxy
 
class  CostmapSwitch
 
class  CpSlamToolbox
 
struct  EvWaypoint0
 
struct  EvWaypoint1
 
struct  EvWaypoint10
 
struct  EvWaypoint100
 
struct  EvWaypoint101
 
struct  EvWaypoint102
 
struct  EvWaypoint103
 
struct  EvWaypoint104
 
struct  EvWaypoint105
 
struct  EvWaypoint106
 
struct  EvWaypoint107
 
struct  EvWaypoint108
 
struct  EvWaypoint109
 
struct  EvWaypoint11
 
struct  EvWaypoint110
 
struct  EvWaypoint111
 
struct  EvWaypoint112
 
struct  EvWaypoint113
 
struct  EvWaypoint114
 
struct  EvWaypoint115
 
struct  EvWaypoint116
 
struct  EvWaypoint117
 
struct  EvWaypoint118
 
struct  EvWaypoint119
 
struct  EvWaypoint12
 
struct  EvWaypoint120
 
struct  EvWaypoint121
 
struct  EvWaypoint122
 
struct  EvWaypoint123
 
struct  EvWaypoint124
 
struct  EvWaypoint125
 
struct  EvWaypoint126
 
struct  EvWaypoint127
 
struct  EvWaypoint128
 
struct  EvWaypoint129
 
struct  EvWaypoint13
 
struct  EvWaypoint130
 
struct  EvWaypoint131
 
struct  EvWaypoint132
 
struct  EvWaypoint133
 
struct  EvWaypoint134
 
struct  EvWaypoint135
 
struct  EvWaypoint136
 
struct  EvWaypoint137
 
struct  EvWaypoint138
 
struct  EvWaypoint139
 
struct  EvWaypoint14
 
struct  EvWaypoint140
 
struct  EvWaypoint141
 
struct  EvWaypoint142
 
struct  EvWaypoint143
 
struct  EvWaypoint144
 
struct  EvWaypoint145
 
struct  EvWaypoint146
 
struct  EvWaypoint147
 
struct  EvWaypoint148
 
struct  EvWaypoint149
 
struct  EvWaypoint15
 
struct  EvWaypoint150
 
struct  EvWaypoint151
 
struct  EvWaypoint152
 
struct  EvWaypoint153
 
struct  EvWaypoint154
 
struct  EvWaypoint155
 
struct  EvWaypoint156
 
struct  EvWaypoint157
 
struct  EvWaypoint158
 
struct  EvWaypoint159
 
struct  EvWaypoint16
 
struct  EvWaypoint160
 
struct  EvWaypoint161
 
struct  EvWaypoint162
 
struct  EvWaypoint163
 
struct  EvWaypoint164
 
struct  EvWaypoint165
 
struct  EvWaypoint166
 
struct  EvWaypoint167
 
struct  EvWaypoint168
 
struct  EvWaypoint169
 
struct  EvWaypoint17
 
struct  EvWaypoint170
 
struct  EvWaypoint171
 
struct  EvWaypoint172
 
struct  EvWaypoint173
 
struct  EvWaypoint174
 
struct  EvWaypoint175
 
struct  EvWaypoint176
 
struct  EvWaypoint177
 
struct  EvWaypoint178
 
struct  EvWaypoint179
 
struct  EvWaypoint18
 
struct  EvWaypoint180
 
struct  EvWaypoint181
 
struct  EvWaypoint182
 
struct  EvWaypoint183
 
struct  EvWaypoint184
 
struct  EvWaypoint185
 
struct  EvWaypoint186
 
struct  EvWaypoint187
 
struct  EvWaypoint188
 
struct  EvWaypoint189
 
struct  EvWaypoint19
 
struct  EvWaypoint190
 
struct  EvWaypoint191
 
struct  EvWaypoint192
 
struct  EvWaypoint193
 
struct  EvWaypoint194
 
struct  EvWaypoint195
 
struct  EvWaypoint196
 
struct  EvWaypoint197
 
struct  EvWaypoint198
 
struct  EvWaypoint199
 
struct  EvWaypoint2
 
struct  EvWaypoint20
 
struct  EvWaypoint200
 
struct  EvWaypoint201
 
struct  EvWaypoint202
 
struct  EvWaypoint203
 
struct  EvWaypoint204
 
struct  EvWaypoint205
 
struct  EvWaypoint206
 
struct  EvWaypoint207
 
struct  EvWaypoint208
 
struct  EvWaypoint209
 
struct  EvWaypoint21
 
struct  EvWaypoint210
 
struct  EvWaypoint211
 
struct  EvWaypoint212
 
struct  EvWaypoint213
 
struct  EvWaypoint214
 
struct  EvWaypoint215
 
struct  EvWaypoint216
 
struct  EvWaypoint217
 
struct  EvWaypoint218
 
struct  EvWaypoint219
 
struct  EvWaypoint22
 
struct  EvWaypoint220
 
struct  EvWaypoint221
 
struct  EvWaypoint222
 
struct  EvWaypoint223
 
struct  EvWaypoint224
 
struct  EvWaypoint225
 
struct  EvWaypoint226
 
struct  EvWaypoint227
 
struct  EvWaypoint228
 
struct  EvWaypoint229
 
struct  EvWaypoint23
 
struct  EvWaypoint230
 
struct  EvWaypoint231
 
struct  EvWaypoint232
 
struct  EvWaypoint233
 
struct  EvWaypoint234
 
struct  EvWaypoint235
 
struct  EvWaypoint236
 
struct  EvWaypoint237
 
struct  EvWaypoint238
 
struct  EvWaypoint239
 
struct  EvWaypoint24
 
struct  EvWaypoint240
 
struct  EvWaypoint241
 
struct  EvWaypoint242
 
struct  EvWaypoint243
 
struct  EvWaypoint244
 
struct  EvWaypoint245
 
struct  EvWaypoint246
 
struct  EvWaypoint247
 
struct  EvWaypoint248
 
struct  EvWaypoint249
 
struct  EvWaypoint25
 
struct  EvWaypoint250
 
struct  EvWaypoint251
 
struct  EvWaypoint252
 
struct  EvWaypoint253
 
struct  EvWaypoint254
 
struct  EvWaypoint255
 
struct  EvWaypoint256
 
struct  EvWaypoint26
 
struct  EvWaypoint27
 
struct  EvWaypoint28
 
struct  EvWaypoint29
 
struct  EvWaypoint3
 
struct  EvWaypoint30
 
struct  EvWaypoint31
 
struct  EvWaypoint32
 
struct  EvWaypoint33
 
struct  EvWaypoint34
 
struct  EvWaypoint35
 
struct  EvWaypoint36
 
struct  EvWaypoint37
 
struct  EvWaypoint38
 
struct  EvWaypoint39
 
struct  EvWaypoint4
 
struct  EvWaypoint40
 
struct  EvWaypoint41
 
struct  EvWaypoint42
 
struct  EvWaypoint43
 
struct  EvWaypoint44
 
struct  EvWaypoint45
 
struct  EvWaypoint46
 
struct  EvWaypoint47
 
struct  EvWaypoint48
 
struct  EvWaypoint49
 
struct  EvWaypoint5
 
struct  EvWaypoint50
 
struct  EvWaypoint51
 
struct  EvWaypoint52
 
struct  EvWaypoint53
 
struct  EvWaypoint54
 
struct  EvWaypoint55
 
struct  EvWaypoint56
 
struct  EvWaypoint57
 
struct  EvWaypoint58
 
struct  EvWaypoint59
 
struct  EvWaypoint6
 
struct  EvWaypoint60
 
struct  EvWaypoint61
 
struct  EvWaypoint62
 
struct  EvWaypoint63
 
struct  EvWaypoint64
 
struct  EvWaypoint65
 
struct  EvWaypoint66
 
struct  EvWaypoint67
 
struct  EvWaypoint68
 
struct  EvWaypoint69
 
struct  EvWaypoint7
 
struct  EvWaypoint70
 
struct  EvWaypoint71
 
struct  EvWaypoint72
 
struct  EvWaypoint73
 
struct  EvWaypoint74
 
struct  EvWaypoint75
 
struct  EvWaypoint76
 
struct  EvWaypoint77
 
struct  EvWaypoint78
 
struct  EvWaypoint79
 
struct  EvWaypoint8
 
struct  EvWaypoint80
 
struct  EvWaypoint81
 
struct  EvWaypoint82
 
struct  EvWaypoint83
 
struct  EvWaypoint84
 
struct  EvWaypoint85
 
struct  EvWaypoint86
 
struct  EvWaypoint87
 
struct  EvWaypoint88
 
struct  EvWaypoint89
 
struct  EvWaypoint9
 
struct  EvWaypoint90
 
struct  EvWaypoint91
 
struct  EvWaypoint92
 
struct  EvWaypoint93
 
struct  EvWaypoint94
 
struct  EvWaypoint95
 
struct  EvWaypoint96
 
struct  EvWaypoint97
 
struct  EvWaypoint98
 
struct  EvWaypoint99
 
class  GoalCheckerSwitcher
 
class  PlannerSwitcher
 
class  Pose
 
struct  Pose2D
 
class  TrajectoryStorage
 
class  WaypointEventDispatcher
 
class  WaypointNavigator
 

Typedefs

typedef smacc2::client_bases::SmaccActionClientBase< nav2_msgs::action::NavigateToPose > Base
 
typedef Base::WrappedResult WrappedResult
 

Enumerations

enum class  SpinningPlanner { Default , PureSpinning , Forward }
 
enum class  Nav2Nodes {
  PlannerServer , ControllerServer , RecoveriesServer , BtNavigator ,
  MapServer , None
}
 
enum class  WaitPoseStandardReferenceFrame { Map , Odometry }
 
enum class  StandardReferenceFrames { Map , Odometry }
 

Functions

geometry_msgs::msg::PoseStamped makePureSpinningSubPlan (const geometry_msgs::msg::PoseStamped &start, double dstRads, std::vector< geometry_msgs::msg::PoseStamped > &plan, double radstep=0.005)
 
geometry_msgs::msg::PoseStamped makePureStraightSubPlan (const geometry_msgs::msg::PoseStamped &startOrientedPose, const geometry_msgs::msg::Point &goal, double length, std::vector< geometry_msgs::msg::PoseStamped > &plan)
 
std::string toString (Nav2Nodes value)
 
Nav2Nodes fromString (std::string str)
 
template<typename TEv >
void configurePostEvWaypoint (std::function< void()> *fntarget, ClNav2Z *client, int index)
 
std::string referenceFrameToString (StandardReferenceFrames referenceFrame)
 

Typedef Documentation

◆ Base

typedef smacc2::client_bases::SmaccActionClientBase<nav2_msgs::action::NavigateToPose> cl_nav2z::Base

Definition at line 28 of file nav2z_client.cpp.

◆ WrappedResult

Definition at line 29 of file nav2z_client.cpp.

Enumeration Type Documentation

◆ Nav2Nodes

enum class cl_nav2z::Nav2Nodes
strong
Enumerator
PlannerServer 
ControllerServer 
RecoveriesServer 
BtNavigator 
MapServer 
None 

Definition at line 29 of file cb_wait_nav2_nodes.hpp.

◆ SpinningPlanner

enum class cl_nav2z::SpinningPlanner
strong
Enumerator
Default 
PureSpinning 
Forward 

Definition at line 53 of file cb_nav2z_client_behavior_base.hpp.

◆ StandardReferenceFrames

Enumerator
Map 
Odometry 

Definition at line 38 of file cp_pose.hpp.

◆ WaitPoseStandardReferenceFrame

Enumerator
Map 
Odometry 

Definition at line 28 of file cb_wait_pose.hpp.

29{
30 Map,
32};

Function Documentation

◆ configurePostEvWaypoint()

template<typename TEv >
void cl_nav2z::configurePostEvWaypoint ( std::function< void()> *  fntarget,
ClNav2Z client,
int  index 
)

Definition at line 1585 of file waypoints_event_dispatcher.hpp.

1586{
1587 fntarget[index] = [=]() { client->template postEvent<TEv>(); };
1588}

◆ fromString()

Nav2Nodes cl_nav2z::fromString ( std::string  str)

Definition at line 117 of file cb_wait_nav2_nodes.cpp.

118{
119 if (id == "planner_server")
120 return Nav2Nodes::PlannerServer;
121 else if (id == "controller_server")
122 return Nav2Nodes::ControllerServer;
123 else if (id == "recoveries_server")
124 return Nav2Nodes::RecoveriesServer;
125 else if (id == "bt_navigator")
126 return Nav2Nodes::BtNavigator;
127 else if (id == "map_server")
128 return Nav2Nodes::MapServer;
129 else
130 return Nav2Nodes::None;
131}

References BtNavigator, ControllerServer, MapServer, None, PlannerServer, and RecoveriesServer.

Referenced by cl_nav2z::CbWaitNav2Nodes::onMessageReceived().

Here is the caller graph for this function:

◆ makePureSpinningSubPlan()

geometry_msgs::msg::PoseStamped cl_nav2z::makePureSpinningSubPlan ( const geometry_msgs::msg::PoseStamped &  start,
double  dstRads,
std::vector< geometry_msgs::msg::PoseStamped > &  plan,
double  radstep = 0.005 
)

Definition at line 37 of file common.cpp.

40{
41 double startYaw = tf2::getYaw(start.pose.orientation);
42 // RCLCPP_INFO(getLogger(),"pure spining start yaw: %lf", startYaw);
43 // RCLCPP_INFO(getLogger(),"pure spining goal yaw: %lf", dstRads);
44 // RCLCPP_WARN_STREAM(getLogger(),"pure spinning start pose: " << start);
45
46 double goalAngleOffset = angles::shortest_angular_distance(startYaw, dstRads);
47 // RCLCPP_INFO(getLogger(),"shortest angle: %lf", goalAngleOffset);
48
49 if (goalAngleOffset >= 0)
50 {
51 // angle positive turn counterclockwise
52 // RCLCPP_INFO(getLogger(),"pure spining counterclockwise");
53 for (double dangle = 0; dangle <= goalAngleOffset; dangle += radstep)
54 {
55 geometry_msgs::msg::PoseStamped p = start;
56 double yaw = startYaw + dangle;
57 // RCLCPP_INFO(getLogger(),"pure spining counterclockwise, current path yaw: %lf, dangle: %lf,
58 // angleoffset %lf, radstep %lf pathsize(%ld)", yaw, dangle, goalAngleOffset, radstep, plan.size());
59 tf2::Quaternion q;
60 q.setRPY(0, 0, yaw);
61 p.pose.orientation = tf2::toMsg(q);
62 plan.push_back(p);
63 }
64 }
65 else
66 {
67 // angle positive turn clockwise
68 // RCLCPP_INFO(getLogger(),"pure spining clockwise");
69 for (double dangle = 0; dangle >= goalAngleOffset; dangle -= radstep)
70 {
71 // RCLCPP_INFO(getLogger(),"dangle: %lf", dangle);
72 geometry_msgs::msg::PoseStamped p = start;
73 double yaw = startYaw + dangle;
74 // RCLCPP_INFO(getLogger(),"pure spining clockwise, yaw: %lf, dangle: %lf, angleoffset %lf radstep
75 // %lf", yaw, dangle, goalAngleOffset,radstep);
76 tf2::Quaternion q;
77 q.setRPY(0, 0, yaw);
78 p.pose.orientation = tf2::toMsg(q);
79 plan.push_back(p);
80 }
81 }
82
83 // RCLCPP_INFO(getLogger(),"pure spining end yaw: %lf", dstRads);
84 geometry_msgs::msg::PoseStamped end = start;
85 tf2::Quaternion q;
86 q.setRPY(0, 0, dstRads);
87 end.pose.orientation = tf2::toMsg(q);
88 plan.push_back(end);
89
90 return end;
91}

Referenced by cl_nav2z::backward_global_planner::BackwardGlobalPlanner::createDefaultBackwardPath(), and cl_nav2z::forward_global_planner::ForwardGlobalPlanner::createPlan().

Here is the caller graph for this function:

◆ makePureStraightSubPlan()

geometry_msgs::msg::PoseStamped cl_nav2z::makePureStraightSubPlan ( const geometry_msgs::msg::PoseStamped &  startOrientedPose,
const geometry_msgs::msg::Point &  goal,
double  length,
std::vector< geometry_msgs::msg::PoseStamped > &  plan 
)

Definition at line 93 of file common.cpp.

96{
97 double dx = 0.01; // 1 cm
98 double steps = length / dx;
99 double dt = 1.0 / steps;
100
101 // geometry_msgs::msg::PoseStamped end;
102 // end.pose.orientation = startOrientedPose.pose.orientation;
103 // end.pose.position = goal;
104 plan.push_back(startOrientedPose);
105
106 // RCLCPP_INFO_STREAM(nh_->get_logger(),"Pure straight, start: " << startOrientedPose << std::endl << "end: " <<
107 // goal);
108 for (double t = 0; t <= 1.0; t += dt)
109 {
110 geometry_msgs::msg::PoseStamped p = startOrientedPose;
111
112 p.pose.position.x = startOrientedPose.pose.position.x * (1 - t) + goal.x * t;
113 p.pose.position.y = startOrientedPose.pose.position.y * (1 - t) + goal.y * t;
114 p.pose.orientation = startOrientedPose.pose.orientation;
115
116 plan.push_back(p);
117 }
118
119 return plan.back();
120}

Referenced by cl_nav2z::backward_global_planner::BackwardGlobalPlanner::createDefaultBackwardPath(), and cl_nav2z::forward_global_planner::ForwardGlobalPlanner::createPlan().

Here is the caller graph for this function:

◆ referenceFrameToString()

std::string cl_nav2z::referenceFrameToString ( StandardReferenceFrames  referenceFrame)

Definition at line 42 of file cp_pose.cpp.

43{
44 switch (referenceFrame)
45 {
46 case StandardReferenceFrames::Map:
47 return "map";
48 case StandardReferenceFrames::Odometry:
49 return "odom";
50 default:
51 return "odom";
52 }
53}

References Map, and Odometry.

◆ toString()

std::string cl_nav2z::toString ( Nav2Nodes  value)

Definition at line 98 of file cb_wait_nav2_nodes.cpp.

99{
100 switch (value)
101 {
102 case Nav2Nodes::PlannerServer:
103 return "planner_server";
104 case Nav2Nodes::ControllerServer:
105 return "controller_server";
106 case Nav2Nodes::RecoveriesServer:
107 return "recoveries_server";
108 case Nav2Nodes::BtNavigator:
109 return "bt_navigator";
110 case Nav2Nodes::MapServer:
111 return "map_server";
112 default:
113 return "";
114 }
115}

References BtNavigator, ControllerServer, MapServer, PlannerServer, and RecoveriesServer.

Referenced by cl_nav2z::CbWaitNav2Nodes::onEntry(), and cl_nav2z::CbWaitNav2Nodes::onMessageReceived().

Here is the caller graph for this function: