SMACC2
Loading...
Searching...
No Matches
Namespaces | Classes | Typedefs | Enumerations | Functions | Variables
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  CbAbortNavigation
 
class  CbAbsoluteRotate
 
struct  CbActiveStop
 
struct  CbLoadWaypointsFile
 
class  CbNav2ZClientBehaviorBase
 
class  CbNavigateBackwards
 
class  CbNavigateForward
 
struct  CbNavigateForwardOptions
 
class  CbNavigateGlobalPosition
 
struct  CbNavigateGlobalPositionOptions
 
class  CbNavigateNamedWaypoint
 
class  CbNavigateNextWaypoint
 
class  CbNavigateNextWaypointFree
 
class  CbNavigateNextWaypointUntilReached
 
class  CbPauseSlam
 
struct  CbPositionControlFreeSpace
 
struct  CbPureSpinning
 
class  CbResumeSlam
 
class  CbRetry
 
class  CbRotate
 
class  CbRotateLookAt
 
struct  CbSaveSlamMap
 
class  CbSeekWaypoint
 
class  CbStopNavigation
 
class  CbUndoPathBackwards
 
struct  CbUndoPathBackwardsOptions
 
class  CbWaitNav2Nodes
 
class  CbWaitPose
 
class  CbWaitTransform
 
class  ClNav2Z
 
class  CpAmcl
 
class  CpCostmapProxy
 
class  CpCostmapSwitch
 
class  CpGoalCheckerSwitcher
 
class  CpPlannerSwitcher
 
class  CpSlamToolbox
 
class  CpWaypointNavigator
 
class  CpWaypointNavigatorBase
 
class  CpWaypointsVisualizer
 
struct  EvGoalWaypointReached
 
struct  EvWaypointFinal
 
struct  NavigateNextWaypointOptions
 
class  Pose
 
struct  Pose2D
 

Typedefs

template<typename TService >
using CbServiceCall = smacc2::client_behaviors::CbServiceCall< TService >
 
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)
 
std::string referenceFrameToString (StandardReferenceFrames referenceFrame)
 

Variables

const std::string frameid = "map"
 

Typedef Documentation

◆ Base

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

Definition at line 28 of file nav2z_client.cpp.

◆ CbServiceCall

template<typename TService >
using cl_nav2z::CbServiceCall = typedef smacc2::client_behaviors::CbServiceCall<TService>

Definition at line 33 of file cb_save_slam_map.hpp.

◆ 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 61 of file cb_nav2z_client_behavior_base.hpp.

◆ StandardReferenceFrames

Enumerator
Map 
Odometry 

Definition at line 37 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

◆ 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 == "behavior_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 "behavior_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:

Variable Documentation

◆ frameid

const std::string cl_nav2z::frameid = "map"