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 |
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
|
strong |
| Enumerator | |
|---|---|
| PlannerServer | |
| ControllerServer | |
| RecoveriesServer | |
| BtNavigator | |
| MapServer | |
| None | |
Definition at line 29 of file cb_wait_nav2_nodes.hpp.
30{
31 PlannerServer,
32 ControllerServer,
33 RecoveriesServer,
34 BtNavigator,
35 MapServer,
36 None
37};
◆ SpinningPlanner
|
strong |
| Enumerator | |
|---|---|
| Default | |
| PureSpinning | |
| Forward | |
Definition at line 53 of file cb_nav2z_client_behavior_base.hpp.
◆ StandardReferenceFrames
|
strong |
◆ WaitPoseStandardReferenceFrame
|
strong |
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}
◆ 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:
